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Motion Planning with Six Degrees of Freedom 

by 

Bruce Randall Donald 

Abstract: The motion planning problem is of central importance to the fields 
of robotics, spatial planning, and automated design. In robotics we are interested 
in the automatic synthesis of robot motions, given high-level specifications of 
tasks and geometric models of the robot and obstacles. The Mover's problem 
is to find a continuous, collision-free path for a moving object through an 
environment containing obstacles. We present an implemented algorithm for the 
classical formulation of the three-dimensional Movers' problem: Given an arbitrary 
rigid polyhedral moving object P with three translational and three rotational 
degrees of freedom, find a continuous, collision-free path taking P from some initial 
configuration to a desired goal configuration. 

This thesis describes the first known implementation of a complete algorithm 
(at a given resolution) for the full six degree of freedom Movers' problem. The 
algorithm transforms the six degree of freedom planning problem into a point 
navigation problem in a six-dimensional configuration space (called C-Space). The 
C-Space obstacles, which characterize the physically unachievable configurations, 
are directly represented by six- dimensional manifolds whose boundaries are five 
dimensional C-surfaces. By characterizing these surfaces and their intersections, 
collision-free paths may be found by the closure of three operators which (i) 
slide along 5-dimensional level C-surfaces parallel to C-Space obstacles; (ii) slide 
along 1- to 4-dimensional intersections of level C-surfaces; and (iii) jump between 
6-dimensional obstacles. 

Implementing the point navigation operators requires solving fundamental 
representational and algorithmic questions: we will derive new structural properties 
of the C-Space constraints and show how to construct and represent C-surfaces and 
their intersection manifolds. A definition and new theoretical results are presented 
for a six-dimensional C-Space extension of the generalized Voronoi diagram, called 
the C-Voronoi diagram, whose structure we relate to the C-surface intersection 
manifolds. The representations and algorithms we develop impact many geometric 
planning problems, and extend to Cartesian manipulators with six degrees of 
freedom. 
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Geometric Planning Problems 

Introduction and Statement of the Problem 

The motion planning problem is of central importance to the fields of robotics, 
spatial planning, and automated design. In robotics we are interested in the 
automatic synthesis of robot motions, given high-level specifications of tasks and 
geometric models of the robot and obstacles. The problem is to find a continuous, 
collision-free path for a moving object through an environment containing obstacles; 
hence it has also been called the Find-Path or Piano Movers' problem. In its most 
general formulation the object can have an arbitrary number of hinges and joints, 
and in some cases coordinated motion planning for multiple objects has been 
considered. We will confine ourselves to the classical formulation of the Movers' 
problem: Given an arbitrary rigid polyhedral moving object P, find a continuous, 
collision-free path taking P from some initial configuration to a desired goal 
configuration. We are particularly interested in the 3-dimensional Movers' problem, 
for an object with 3 translational and 3 rotational degrees of freedom. This thesis 
describes the first known implementation of a complete algorithm (at a given 
resolution) for the full 6 degree of freedom Movers' problem. 

1.1. What are Geometric Planning Problems? 

Our work has impact on a class of geometric planning problems. In robotics 
we arc typically interested in motion planning for a mobile robot or manipluator. 




Figure 1.1. An example of a solution path for the classical Mover's problem with six degrees 
of freedom. This illustration is a "time-lapse" picture of a path found by our planner for a 
hammer-shaped object. In all our examples, the workspace is bounded by a box (which is not 
shown). This solution path requires use of all three rotational degrees of freedom. 

In Computer-Aided Design (CAD), the problem of automated structural design 
for n structural members is also an instance of the most general form of the 
Mover's problem. The problem of determining whether an object can be assembled 
as designed, and of generating an assembly plan if it can, is also in this class. 
Examples of geometric planning problems include: 



(i) The Find-Path or Movers 'problem is to find continuous, collision-free path 
for one or more moving objects in the presence of obstacles. Find-path problems 
fall into two broad categories: single-body and multiple-body motion planning 
problems. In the classical Movers' problem, a single rigid polyhedral object must 
be moved through a workspace containing polyhedral obstacles. For the linked 
or hinged body Movers' problem, a set of moving objects connected via joints or 
linkages must be moved. An industrial robot arm is a typical example of such an 




Figure 1.2. A different view of the solution path for the hammer example, with the obstacles 
"transparent" to allow us to view the rotations better. 

object. In the coordinated planning problem, a number of independent (i.e., not 
necessarily linked) objects must be moved. An algorithm for multiple body motion 
planning must ensure that the moving objects collide neither with the walls nor 
with each other. 

(ii) The find-space problem is to find a collision- free placement for one or 
more objects in a field of obstacles. By analogy with the find-path problem, we 
can speak of the classical, linked-body, and coordinated find-space problems. In 
computer-aided design and automated design, the find-space problem is typically 
subject to additional geometric constraints. Lozano-Perez (1983) grouped find-path 
and find-space algorithms together as the spatial planning problems. 

(Hi) The fine-motion problem entails motion-planning along obstacle surfaces, 
typically while maintaining some applied force. Collision-free paths and placements 
avoid obstacles: however, for many tasks in robotics and in automated design, it 




Fi K ure 1.3. Application Example: Planning for an industrial robot arm with sbc degree. .of 
freedom is an example of the linked-body movers' problem. (F^ ^ou^yjfRo^B^ 

is necessary to plan motions and placements in contact with obstacle surfaces. For 
example, consider the tasks of welding, insertion, and assembly in robotics. These 
tasks require compliant motions, entailing consideration of additional physical 
constraints such as friction, kinematics, and force control. However, the compliant 
motion planning problem has a strong geometric flavor and its solution requires 
the tools of spatial planning (see Mason (1981), Erdmann (1984)). 

(iv) Recently, researchers have begun to consider motion planning with 
uncertainty (Mason (1981), Brooks (1982), Lozano-Perez, Mason, and Taylor (1983), 
Erdmann (1984)). Broadly speaking, uncertainty may arise from inaccuracy in 
object models, sensors, or control. Motion planning with uncertainty also presumes 
algorithms and representations from spatial planning. 

As we can see, all geometric planning problems contain components of the 
spatial planning problem, especially if the underlying geometries are the same. In 
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Figure 1.4. Example from computer-aided design: Automatically, generated flat-plate structure 
from Donald (1983b). How can wc generate structural patterns subject to the constraints of the 
building envelope and mechanical core? 

particular, for high-dimensional configuration spaces, the theoretical analyses of 
Mason (1981), Lozano-Perez, Mason, and Taylor (1983), and Erdmann (1984) all 
presume geometric results which are derived in this thesis. 

This work impacts all geometric planning problems. To illustrate the theoretical 
results, we address one particular problem, namely the classical Movers' problem 
with six degrees of freedom. Our algorithms immediately generalize to applications 
involving gross-motion and fine-motion planning for Cartesian manipulators with 
six degrees of freedom. 
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Figure 1.5. A find-path problem for an L-shaped object. The L-shaped object is shown amidst 
obstacles in the start and goal configurations. ^^^ 
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Figure 1.13. Solution Path 1, frame 64 (final configuration). 
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Figure 1.6. Solution Path I, frames 1-9: A difficult solution path for the L-shaped object. 
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Figure 1.7. Solution Path I, frames 10-18 
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Figure 1.8. Solution Path I, frames 19-27 
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Figure 1.9. Solution Path I, frames 28 36 
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Figure 1.10. Solution Path I, frames 37-45 
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Figure 1.11. Solution Path I, frames 46-54 
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Figure 1.12. Solution Path I, frames 55-63 
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Figure l.M. A different view of the same solution path, showing how the L-shaped object must 
rotate to attain the final position. The first six frames are not shown, since the moving is not 
visible from this perspective. Solution Path 1 (view 2); frames 7 15 
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Figure 1.15. Solution Path I (view 2); frames 16-24 
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Figure 1.16. Solution Path I (view 2); frames 25 33 
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Figure 1.17. Solution Path I (view 2); frames 34-42 
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Figure 1.18. Solution Path 1 (view 2); frames 43 51 
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Figure 1.19. Solution Path I (view 2); frames 52 60 
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Figure 1.20. Solution Piith I (view 2); frames 61-64 
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Figure 1.21. A detail of the path for the L-Shaped example. The detail ia in "time-lapse" format, 
and shows a complex double rotation near the goal configuration. 

Examples of "classical" find-path problems solved by our planner may be found 
throughout this chapter, and also at the end of chapter 2 (section 2.4, "Examples 
of the Local Experts in Use"). See fig. 1.5-21, 1.22-28, and 2.7-21. In general, 
geometric planning problems with more than three degrees of freedom have proven 
extremely difficult to solve. We believe that in part, this difficulty has been due 
to the unresolved issues in the mathematics of spatial planning. By solving these 
problems for the six degree of freedom case, and illustrating the results for the 
find-path problem (which holds considerable intrinsic interest), we hope to provide 
a geometric foundation which will make all geometric planning problems feasible. 
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Figure 1.22. (3 Views of the "Puzzle Problem"). In this find-path problem, the L-shaped object 
must be moved around the diagonally-placed obstacle. Here the L-shaped object is shown in the 
initial and goal configurations. 

1.2. A Simple Example: How to Find a Path for a Point Amidst 3-D 

Polyhedral Obstacles 

We will begin by discussing an algorithm for navigating a single point amidst 
polyhedral obstacles in three-dimension Euclidean space 3? 3 . We then review the 
configuration space transformation of Lozano-Perez (1983), which transforms the 
problem of reasoning about the motion of a polyhedral object to reasoning about a 
single point in configuration space. If the configuration space is isomorphic to 3R , 
then the point navigation algorithm can be applied directly to find collision free 
paths. In this thesis we will generalize the point navigation algorithm to work in 
the configuration space for a three dimensional polyhedral object with six degrees 
of freedom. 

The six degree of freedom planner is based on the following analogy: suppose 
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Figure 1.23. Puz/lc Problem, frames (1-6), view 1. 
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Figure 1.24. Puzzle-Problem, frames (7-12), view 1 
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Figure 1.25. Puzzle-Problem, frames (1-6), view 2 
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Figure 1.26. Puzzle-Problem, frames (7-12), view 2 
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Figure J. 27. Puzzlc-Problctn, frames (1-6), view 3 
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Figure 1.28. Puzzle-Problem, frames (7-12), view 3 
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we wish to find a path for a point in 5ft 3 , avoiding collisions with polyhedral 
obstacles, where each polyhedron is modeled as the intersection of a finite number 
of half-spaces of 5ft 3 . One solution might be to move until the point comes in 
contact with a polyhedron, and then to move around the obstacle by traversing the 
edge-graph on its boundary. (Refer to figure 1.39, ignoring the caption for now). 
Each arc in the edge-graph is the intersection of two surfaces bounding half-spaces. 
Even if the polyhedra are allowed to overlap, the technique will still work since their 
intersections have the same structure. Naturally, we will also need some technique 
for jumping from one obstacle to another. 

To summarize, we can find a collision-free path for a point amidst obstacle 
polyhedra in 5ft 3 through the closure of three operators: 

The Point Navigation Operators 



Operator (i) slides along the 1-dimensional edges, which lie in the intersections 

of the obstacle planes; 

Operator (ii) slides along the two dimensional obstacle planes, which contain 

faces of the obstacle polyhedra; 

Operator (iii) jumps from one 3-dimensional obstacle to another. 



We now review the configuration space transformation. Using this transfor- 
mation in its simplest form, the find-path problem in three dimensions without 
rotations is reduced to the point navigation problem amidst polyhedral obstacles. 

1.3. Configuration Space 

The configuration of a moving object is a vector of parameters representing 
its combined translation and orientation, relative to a specified coordinate system. 
For the classical Movers' problem in the plane, a typical configuration 



{x,y,0) 
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represents a displacement (translation) of (x, y), and a rotation by 0. (For example, 
imagine a polygon displaced by (x,y), and rotated by about one of its vertices). 
For the six degree of freedom classical Movers' problem, a typical configuration 

X = (x,y,z,£(0)) 

represents a displacement (translation) of (x,y,z), and a three dimensional rotation 
£(6). The three dimensional rotation group is a three parameter family; typical 
representations of rotations include Euler angles, (Symon (1971)), spherical angles, 
and quaternions (Hamilton (1969)). For example, if the Euler angles 6 = (ip, 0, (j>) 
are employed, then they determine a 3 by 3 rotation matrix which functions as 
£(©) in the rotation group. It is convenient to identify the rotation operator with 
its parameterization, that is, to express X as 

X —■ (x,y,z,ip,0,ct>). 

Using configuration space, reasoning about the motion of a complicated three- 
dimensional body amongst obstacles may be transformed into reasoning about 
a point in a six dimensional configuration space. The transformation described 
by Lozano-Perez (1983) entails "shrinking" the moving object to a point, and 
correspondingly "growing" the obstacles. In principle, the point may then be 
navigated around the grown obstacles by means of the point navigation operators 
(above). 

In this thesis, the point navigation operators will be generalized to the 
six-dimensional configuration space of the classical Movers' problem. 

In order to present our algorithm for planning in C-Space, it is necessary to 
review the basics. We present an introduction to representations in configuration 
space at two levels: first, we present an intuitive discussion. Next, we present a 
more detailed, slightly more mathematically-oriented exposition. For the sake of 
readability, there is some redundancy in the sections. Those who are encountering 
configuration space for the first time may wish to postpone reading the latter 
section for now. 
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Figure 1.29. These figures show an obstacle polygon B and a moving polygon A. A must be 
moved around B to the goal configuration. 7 



1.3.1- Representations in Configuration Space: An Intuitive Treatment 

Figure 1.29 is an example of the classical Movers' problem in two dimensions, 
without rotations. A is a moving object which must be moved from the start 
configuration to the goal configuration, around an obstacle polygon B. The start 
and goal configurations may be expressed as two dimensional vectors of the form 
(i, y) which represent the displacement of a vertex u_ on A from a fixed coordinate 
frame. The displacement is a rigid translation of the polygon A. The C-Space of 
this Movers' problem is the space of two dimensional translations, which is the 
same as the Cartesian plane. Lozano-Perez (1983) demonstrated a transformation 
which shrinks A to the vertex v lt while inversely growing B. The grown obstacle for 
B is a C-Space obstacle called CO{B), and is shown in figure 1.31. (We will discuss 
the details of this transformation later). The problem of moving the polygon A 
from the start to the goal is transformed into the problem of navigating the point 
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Figure 1.30. The edges and vortices of A and B. 

v\ around the C- Space obstacle shown in figure 1.31. 

Both A and B are convex; non-convex objects are represented by overlapping 
unions of convex polyhedra. The C-Space obstacle CO(B) is constructed by 
considering all feasible interactions of the edges and vertices of A and B. Each such 
interaction generates a constraint which is manifest as an edge of CO(B). We say 
that an interaction between a vertex of A and an edge of B, or between an edge of 
A and a vertex of B, is feasible if there is some pure translation which can bring 
the vertex and edge in contact without causing A and B to overlap. For example, 
the set of all possible interactions of A and B is the union of the two cartesian 
products 



{ei,e2,e 3 } X {v[, v' 2 , v^,v' 4 } 



and 
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Figure 1.3 L. CO(S), the grown obatacle for B in C-Space. The vertex w x muat be navigated 
around CO{B). 



{vi,V2,v 3 } X {e[,e' 2 ,ez,e' A }. 
However, at the depicted orientation of A, only these interactions are feasible: 



{ (v lf e[), {v u efi, (d, "3). («2, «3)» ( e 2, «J), (« 3 , ej), (e 3 , «{) }• 

It is easy to visualize the translation that will bring any of these pairs into contact. 
Furthermore, note that (for example), no translation can bring v 3 in contact with 

Now imagine that A is allowed to rotate about v\. At different orientations, 
there will be different sets of feasible contacts. We say that the constraints associated 
with feasible contacts are applicable constraints. It should be clear that at any given 
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Figure 1.32. CO{D) can be represented as the intersection of 7 half-spaces, whose boundaries 
contain edges bounding CO(B). 

orientation, only certain constraints will be applicable. The orientations for which a 
given constraint is applicable form its applicability region. The applicability regions 
for each constraint in this problem are angular sectors of the form [0/ < < 9 U \. 
This simply means that there exists a range of angles in which a particular contact 
is feasible. This range of angles may be geometrically visualized as a sector of a 
circle. 

When A is allowed to rotate, the geometry of CO{B) changes as varies, and 
as the set of applicable constraints changes. As the edges and vertices of A rotate 
about vi, the constraints they generate sweep out ruled surfaces which bound 
CO{D) in a three dimensional C-Space. (The C-Space is three dimensional, since 
A now has three degrees of freedom: x, y, and 0). At any fixed orientation O , an 
x-y slice of CO{D) is a polygon, called slice{CO{B),0 o ). Figure 1.31 shows such a 
slice at the depicted orientation of A. With each edge of slice{CO{B), 0) there is an 
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associated half-plane containing slice(CO(B), 0), whose boundary contains the edge 
(see figure 1.32). The intersection of these half-planes is exactly slice(CO(B),6). 
As changes, different half-planes are used to construct slice(CO(B),Q). By (1) 
deriving the line equation of the boundary of these half-planes in terms of the 
orientation 6, and (2) by determining the applicability region for each half-plane 
constraint, we can characterize slices of the C-Space obstacle slice(CO(B), 6) as 6 
changes. Thus we can characterize the three dimensional C-Space obstacle CO(B). 
This representation may be used to develop planning algorithms for the Movers' 
problem with two translational and one rotational degrees of freedom (see Brooks 
and Lozano-Perez (1983)). 

In this thesis we develop such a representation for the six degree of freedom 
Movers' problem. There are several problems which must be solved. Because 
the structure of the rotation group is more complicated in three dimensions, 
the applicability regions for constraints in a six degree of freedom C-Space are 
geometrically much more complicated. While in two dimension* the applicability 
regions may be visualized as sectors on a circle, in three dimensions they are 
complicated three dimensional manifolds on the projective 3-sphere. It is important 
to characterize these regions, since they specify where a constraint is applicable. 
We will discuss some of the other problems presently. 

Generalizing the Point Navigation Operators Requires Solving 
Representational Questions 

In order to generalize the point navigation operators to the C-Space of the 
classical Movers' problem, we must be able to characterize the surfaces of C-Space 
obstacles, and the intersections of these surfaces. The first two operators, then, must 
slide along the C-surfaces and their intersections. In the next section, we discuss some 
of the representational issues involved in developing such operators. For example, 
when rotations are allowed, the C-surfaces are curved. In the six-dimensional space 
of the classical Movers' problem, each C-surface is a five-dimensional submanifold of 
C-Space, and the intersection of two such surfaces is a four- dimensional manifold. 
Thus it is possible to slide along such an intersection with four degrees of freedom. 
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1.3.2. Representations in Configuration Space: A More Formal Treatment 

In this section, we present a somewhat more abstract formulation of 
representational issues in C-Space. Some readers may wish to postpone reading this 
section until later. We will proceed as follows: first, we will outline an important 
representational question which must be solved in this thesis. Next, we discuss 
how to represent volumes (such as C-Space obstacles) in C-Space. In the course 
of this discussion, several terms will be defined in context by means of intuitive 
descriptions. At the end of this section, under the heading Working Definitions, we 
will summarize and formalize the definitions to the extent that will be required in 
chapters 1 and 2. 

The Domain Question 

Until now, geometric planning problems with more than three degrees of 
freedom have proved resistant to solution. 1 In this thesis we provide such an 
algorithm for find-path with six degrees of freedom (the classical Movers' problem). 
The resistance of these problems has largely been due to unresolved mathematical 
issues and questions relating to the structure of configuration space and to the nature 
of C-Space constraints, (although for fine-motion and planning with uncertainty 
there are of course additional issues). 

One fundamental theoretical problem for high-dimensional configuration spaces 

may be stated as follows: in a configuration space C with rotations, each C-Space 

obstacle may be represented by the intersection of a finite number of half-spaces. 

Each half-space, in turn, is defined by a real-valued C-function on C-Space. For 

example, the half-space might be the set of configurations where the C-function 

is negative. However, each C-function is a partial function on C, whose domain 

is a complicated region in C-Space. This greatly complicates the representation 

for C-Space obstacles and C-surfaces (see figure 1.36). Moreover, until now the 

domains of the C-functions were unknown for all but the one-dimensional rotation 

group. One of our first tasks will be to derive the domains of all C-functions for 

the classical Movers' problem with six degrees of freedom. 

'However, previous work has provided an existence proof of a polynomial time algorithm for 
certain spatial planning problems. In addition, there arc approximate algorithms for some of these 
problems. See our review of previous work. 
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There are several related problems, for which we also present solutions. This 
allows us to construct a complete geometric representation for the configuration space 
of the classical Movers' problem with six degrees of freedom. This representation 
impacts all the geometric planning problems we have discussed, and extends 
naturally to Cartesian Manipulators. 

Representing Volumes in Configuration Space 

The dimensionality of configuration space is the number of degrees of freedom 
in the parameter space, i.e., the number of degrees of freedom available to 
the moving object(s). Thus the classical Mover's problem in the plane has two 
translational and one rotational degrees of freedom, while in three dimensions it 
has three translational and three rotational degrees of freedom. The configuration 
spaces for these problems are three and six dimensional manifolds, respectively. 
As the number of degrees of freedom increases, a geometric planning problem 
becomes harder. There are several reasons for this. First of all, when rotations are 
allowed, configuration space ceases to be Euclidean, and the C-surfaces become 
curved. Furthermore, the non-commutativity and multiple-connectedness of the 
three-dimensional rotation group are classical difficult issues in mathematics. In 
addition, it can be shown that the computational complexity of spatial planning 
grows exponentially with the dimensionality of the C-Space. 

A fundamental issue for geometric planning algorithms is: how should C-Space 
obstacles and surfaces be represented and computed? 

A volume in a configuration space C may be represented by the intersection of 
a finite number of half-spaces (see figure 1.33). Each half-space may be defined via 
some smooth, real-valued function function on C, 

fi'.C-* 5R. 

For example, (fig. 1.34) suppose fi(x,y) = ax + by + c, for some constants a, b, 
and c. The kernel of / is the line where f(x, y) = 0. The halfspace hj is the 
portion of the plane where f(x,y) is negative. C-functions such as /, arise in the 
two dimensional Movers' problem, at a fixed orientation. 
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Figure 1.33. The region CO is the intersection of the half-spaces h t , fty , h k , and h t . 

In general, the (closed) half-space hj is the set of all points in C where /,- is 
negative- valued or zero: 



hT = {veC\fi(v)<o}. 

The common intersection of a number of such half-spaces can yield a volume in 
C. Lozano-Perez (1983) showed how C-Space obstacles can be modeled in this 
manner, and gave the form of the functions /,-. Note that each C-surface lies 
within the kernel of some constraint /;. Fine-motion strategies and algorithms for 
planning with uncertainty need to compute the normals and tangent spaces to 
these C-surfaces. The normal can usually be derived from the gradient V/,- (this 
requires placing an appropriate Riemannian metric on the tangent space). When 
a real-valued function /,• on conCguration space is used to describe constraints in 
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Figure 1.35. Illustration of the classical Mover's problem in three dimensions. B is an obstacle, 
and A is an object which must be moved around D. A,# t shows A in the start configuration, 
and A gt & 2 shows A in the desired goal configuration. C is the polyhedron which is the C-Space 
obstacle from 1) for A at orientation ©i. At orientation ©', the C-Space obstacle from B is a 
different polyhedron, which we show as C 
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Example: (See figure 1.35). Consider the Movers' problem for a three dimensional 
polyhedron A which can translate but not rotate amidst polyhedral obstacles. The 
configuration space for A is a three-dimensional vector space of translations, which 
it is convenient to identify with 3?'*. Each constraint /,- will be linear on 5R , and 
the kernel of / t - is a plane. Each such plane bounds a C-Space obstacle (such as C 
or C' in figure 1.35). The C-Space obstacles are possibly overlapping polyhedra in 
3? 3 . The find-path problem in the transformed space is that of navigating a point 
past the union of these C-Space polyhedra (see figure 1.39). 

In this formulation there is a fundamental underlying assumption: f{ is a total 
function on C, that is, the domain of / t - is all of C. In the example this is not a 
problem, since the domain of each linear C-function is the entire space. A function 
whose domain is a subset of C is called a partial function on C. When rotations are 
allowed, C-functions become partial functions. 
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Figure 1.36. The functions /,, /*, and f k are used to describe the hatf-spaccs h~, hj, and 
h±. If all functions arc total functions on the plane, then the intersection of the corresponding 
half-spaces will be the the rectangloid region R. However, suppose that /,• is a partial function, 
whose domain is restricted to the half -space where y is positive. Wc say that /, is not applicable 
below the line y = 0. Furthermore, wc assume that points outside the domain of fi arc within 
h~~. In this case, the intersection /i," f) hj f\ /tj~ is triangloid region T. 
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Figure 1.37. The two-dimensional classical Movers' problem: An pbstncle polygon B and a moving 
polygon A. A is shown at a particular orientati on, tfi. 

Why do C-Space constraints become partial functions when rotations are 

introduced? Consider the classical find-path problem in two dimensions, for a 

moving polygon A which can translate and rotate in the plane (see figure 1.37). A 

configuration of A may be represented by three parameters, (i, y, 6). The surfaces of 

the C-Space obstacle for B arise from each of the feasible contacts (or interactions) 

between the edges and vertices of A and B. Thus the constraint functions (which 

we have been calling /,-) are defined by considering pairwise interactions of edges 

and vertices of A with vertices and edges of B. Every such pair such as e a and v b 

will generate a smooth, real-valued C-function f ea , Vb on configuration space. 2 Each 

constraint is designed such that their conjunction enforces a disjointness criterion 

for A and B. However, not all interactions are possible at any given orientation. 

For example, at the depicted orientation 0, of A, edge e 2 can interact with vertex 

v, but edge e, cannot: at orientation 0,, no translation can bring e x in contact 

2 1' or the form of tlie C-fu notions, see chapters 3 and 4. 
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with v while maintaining the disjointness of the interiors of A and B. We say the 
associated C-function / ei)V is not applicable at orientation 9\. In other words, no 
configuration 

is in the domain of f euV . Each constraint f Ca>Vb is applicable only at certain 
orientations, and hence each can be considered a partial function on the C-Space. 

In three dimensions (see figures 1.35 and 1.39), the surfaces of the C-Space 
obstacle for B arise from each of the feasible contacts between the vertices, edges, 
and faces of A and B. By analogy with figure 1.37, it is clear that not all of these 
interactions are feasible at any given orientation. Thus the C-functions describing 
C-surfaces for spatial planning with six degrees of freedom must also be partial 
functions. 

Working Definitions: Review and Summary 

We now summarize and formalize the key definitions and concepts required in 
this chapter and the next: 

Configuration Space: (Formal definition) Configuration space is the product 
space of the space of translations and the space of rotations for an object. In three 
dimensions, the space of translations is Euclidean 3-space 5ft 3 and the space of 
rotations is the 3-dimcnsional rotation group or Special Orthogonal Group, SO(3). 
50(3) is isomorphic to the intersection of the Special Linear Group (the set of 
all real 3X3 matrices with determinant 1) and the Orthogonal Group (which 
may be thought of as the set of matrices with orthonormal rows and columns). 
The orthonormality of rows and of columns are equivalent conditions. 50(3) is 
isomorphic to P 3 , the 3-sphere S 3 with opposite points identified. (P 3 is also known 
as the projective 8-sphere). S 3 is isomorphic the group of unit quaternions. For the 
classical Movers' problem we will employ configuration space, 3?'* X 50(3). We will 
denote the classical Movers' Problem with three translational and three rotational 

3 Sec Brooks and liOsano-l'ertVz (1983) Tor a, discussion of the domains of C-functions for the two 
dimensional find-path problem with rotations. 
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degrees of freedom as 6DOF. In practice, we will represent rotations as members 
of a three-parameter family (for example, Euler Angles), but we must keep in mind 
that they parameterize an isometry and that 3? 3 X SO[3) is not a vector space. 
If the Euler angles (t/>,0, ^) are employed to represent the orientation of a rigid 
polyhedral body, a typical configuration X in D? 3 X 50(3) has the form 

X = {x,y,z,ij),6,(S)). 
We will sometimes adopt the notation 

x = (r,e), 

where r denotes a three-dimensional translation vector, and © some three 
dimensional rotation. This second notation is independent of the particular 
representation chosen for rotations; the first isn't. If Euler angles are employed, we 
may think of G as the "vector" of Euler angles, (ip, 6, </>). 

C-Space Obstacle: (Informal definition) Configuration space obstacles are 
(possibly overlapping) six dimensional manifolds (with boundary) which correspond 
to sets of configurations that would cause collisions of the moving object with real 
space obstacles. 

Free space: The free space is that subset of C-Space which lies within no 
C-Space obstacle. The free space will be denoted F. 

Applicability Set: (Informal definition) Refer to figures 1.29-32 and 1.33-38, and 
recall that C-Space obstacles are represented by the intersection of a finite number 
of half-spaces. (To be formal we should call them half hyperspaces). The boundary 
of each half-space is a C-surface, and contains a boundary patch of the C-Space 
obstacle. Each C-surface S may be expressed as the kernel of a real- valued function 
/ on C-Space. The C-function f is negative on the obstacle side of the half-space 
C-Space obstacle, and positive on the other half. In the literature C-functions have 
been called constraints, since they express constraints on the possible motions for 
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an object. A surface parallel to S is called a level C-surface, and represents the set 
of configurations where / has a certain fixed value. This value is termed the level 
of the level C-surface. The boundary of the C-Space obstacle is a special case of 
level C-surface, where the level is zero. We have seen that at any given orientation, 
only certain C-functions (and their associated C-surfaces) are applicable. This is 
because only certain contacts are feasible between the faces, edges, and vertices 
which generate the C-functions. We call this set of C-functions the applicability set. 
For example, in 1.29-32, at the depicted orientation the applicability set is 

{ (ui, e[), {v\,e' 2 ) t {e x , u£), (v 2 , e£), (e 2 , v[), (« 3 , e[), (c 3 , v[) }. 

(Actually, the applicability set is the set of C-functions generated by these 
vertex/edge and edge/vertex pairs, but since there is a one-to-one correspondence 
between the generator pairs and the C-functions, we can write it this way). In later 
chapters, we will demonstrate algorithms for computing the applicability set, and 
for decomposing rotation space into regions where the applicability set is invariant. 

Redundant and Non-redundant Constraints: (Informal definition). If a config- 
uration X is in free space, the set of constraints which is (locally) relevant to motion 
planning from X is a subset of the applicable, positive-valued C-functions at X. 
However, the value of a C-function does more than merely indicate which side of a 
C-surface X is on. A C-function's value represents the translational distance to that 
surface. Thus, C-functions provide a collection of pseudo-metrics on C-Space. Using 
these metrics, it is possible to order C-surfaces by their closeness to a configuration 
X (simply sort the C-functions on their value at X). We say that a C-surface is 
redundant if it is subsumed by a nearer, intervening constraint. In figure 1.38, for 
example, / and g are non-redundant constraints at X, but h is redundant since it 
is subsumed by /. It is useful to determine the set of non- redundant constraints at 
X since this is the smallest set of constraints that are locally relevant to motion 
planning. We provide a formal definition of redundancy in chapter 3. 

Robot, Moving Object, and Piano: All of these terms have been employed in 
the literature to refer to the moving object for which a collision-free path must 
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Figure 1.38. ft is a redundant constraint. 



be found. Our moving object is modeled as the possibly overlapping union of a 
finite set of convex polyhedra. The union is rigid, but not necessarily convex or 
connected. The moving object has three translational and three rotation degrees of 
freedom. To avoid monotony, we may employ the term robot to refer to the moving 
object. The terminology is justified in part by the fact that our algorithm extends 
straightforwardly to Cartesian manipulators. 
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The Research Agenda of this Thesis 
A Brief Outline 



I. Computational Theory 



Paths can be found in C-Space by the closure of three operators: 
(i) slides along 1- to 4-dimensional intersections of level C-surfaces; 
(ii) slides along 5-dimensional level C-surfaces; 
(iii) jumps between 6-dimensional obstacles. 



II. Representation and Algorithm 



Search Algorithm Employing the Three Operators 



tr 



Solve the Intersection Problems. 

Develop a represention for the intersection manifolds. 



tr 



Solve open questions about the structure of 6DOF constraints. Derive and 
represent structural properties of the constraints, for example, the domains of 
defining partial functions. Develop decomposition algorithms. 



ID. Implementation 



Implement the 6DOF planner. 



IV. New Theoretical Results 



The structure of 6DOF constraints: Theorems on the domains and domain 

topology of the defining partial functions. 

Theorems on the applicability decomposition. 

The C-Voronoi Diagram (CVD). 

The Equivalence Theorem for intersection manifolds and the CVD. 

Criteria for designing/integrating local and global planning algorithms. 
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1.3.3. Generalizing the Point Navigation Operators 

Consider a three-dimensional configuration space containing smooth, curved 
C-Space obstacles. Observe that the point navigation operators will work even if 
the surfaces are curved and complicated, as long as we can find their intersections. 
For the two-dimensional Mover's problem (for a polygon allowed to rotate and 
translate in the plane), we employ a configuration space 5ft 2 X S l . 5ft is the space of 
two dimensional translations, and S x is the unit circle, on which one-dimensional 
rotations may be represented. C-functions are of the form /,- : 5ft 2 X S l -> 5ft and 
are valid within some sector A{ of S 1 . A C-surface is the set of configurations 
where / t - is zero. Although these C-functions are complicated expressions containing 
trigonometric terms of the form zcosfl and ys'mO, it is possible to solve two 
such C-surfaces simultaneously to obtain an intersection curve in 5ft 2 X S which 
is parametric in 6 (these intersection manifolds are derived in chapter 4). The 
analogy to navigating a point through a polyhedral environment should now become 
clear: the faces of the polyhedra correspond to C-surfaces in 5ft 2 X S 1 and the 
edge-graphs to the graph of C-surface intersections. By searching the graph of 
C-surface intersections we can find a path in configuration space, if one exists. 

Planning in a Six Dimensional C-Space 

Our planner for a six dimensional C-Space is based on the idea of moving along 
the intersections of level C-surfaces in free space, parallel to the boundaries of C- 
Space obstacles. In the example above, the coincidence between the dimensionality 
of configuration space and Euclidean space was serendipitous: edges on polyhedra 
corresponded to curves in 5ft 2 X S 1 , and faces to 2-dimensional surfaces. However, in 
a six dimensional C-Space, the C-surfaces are 5-dimensional and their intersections 
are 4- dimensional sub-manifolds. Intuitively this means that the set of possible 
motions while complying with two constraints is a four-parameter family. 

Our idea is as follows: Suppose we could slide along C-surfaces (see figure 1.39). 
In addition, suppose we could intersect C-surfaces to construct a lower dimensional 
manifold in C-Space which contained paths along (or around) the boundary of 
C-Space obstacles. By sliding along C-surfaces, and by sliding along the intersection 
of C-surfaces, we should be able to devise an algorithm which can circumnavigate 




Figure 1.39. We can represent the configuration of a polyhedron A by a pair, (T, ©), where T is 
a translation of A and 9 is a rotation of A. The problem of moving A from configuration (r„ ©i) 
to (r e ,0 2 ) is transformed to the problem of navigating a configuration point, r, past C, which 
is the C-Space obstacle due to B. 5 X and S 2 are C-surfaces bounding C. The configurations c t - 
lie on the boundary of C, while d x is in free-space. Two trajectories around B are shown. Note 
that the path segments (c 6) (r ff ,0 2 )) and [di,{r g , 6 2 )) must also include a rotation. (The actual 
reference point is at the centroid of A, but for the purposes of exposition, we have placed it at a 
vertex as shown). 

C-Space obstacles. (Of course, we also need a way to plan motions which "jump" 
from one obstacle to another). 

Example: Figure 1.39 shows how such a planning algorithm might work. The 
planner moves through free-space from (r s ,0i) until it strikes a C-surface Si at 
ci- From c\ a path is found towards ci sliding along the C-surface S\. We say the 
planner slides along S\ to C2- Configuration C2 lies on an intersection manifold of 
the C-surfaces S\ and 5 2 . The path segment (c2,ca,) slides along this intersection 
manifold, which lies on the boundary of C. A path (02, C3, c 4 , C5, c c ) is planned along 
the graph of intersection manifolds on the boundary of C. From eg we leave the 
boundary of C, and translate and rotate through free-space to (r g ,©2). This path 
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is an idealized example of planning along C-surfaces and intersection manifolds of 
C-surfaces. The implemented planner finds a path similar to ((r a , 9i), d\,{r g , Q2)) 
(see chapter 2). The path segment {(r a ,Q),di) is parallel to the C-surface S\, and 
we say that it slides along a level C-surface for Si. The path segment (di,(r g ,Q)) 
is along a level C-surface for 5 2 . These level C-surfaces intersect along a manifold 
in free-space containing di (imagine extending the faces Si and S2 beyond the 
boundary of C until they intersect). 

We will derive the necessary mathematical theory and tools relating to C- 
surfaces and their intersection manifolds, and present algorithms for moving and 
planning paths in C-Space. Some of the issues we will address include: 
(i) What is an appropriate representation for constraints in a six dimensional 

C-Space? 
(ii) How do we plan motions using constraints whose domains change with the 

motions? 
(iii) How can trajectories in C-Space be intersected with C-surfaces whose domains 

change along the trajectory? 
(iv) How can intersection manifolds be constructed in C-Space? 
(v) How are motions planned which slide along C-surfaces and intersection manifolds? 

It is useful to develop a terminology for evaluating algorithms and repre- 
sentations for geometric planning problems. An algorithm employing an approximate 
representation does not characterize the constraints exactly. A complete algorithm 
(for a given resolution) is guaranteed to find a solution if one exists (at that 
resolution). In general, construction of a complete algorithm mandates the employ- 
ment of a complete representation. A brute-force algorithm tries to find a solution 
through exhaustive search. Heuristic algorithms fall into two (overlapping) classes: 
heuristically complete, and heuristically fast. See also the review of previous work 
(below). 

The implemented algorithm we present for the classical Mover's problem with 
six degrees of freedom employs a complete representation of the configuration space 
constraints, and a complete search algorithm (for a given resolution). 
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1.4. Local versus Global 

Local algorithms for the find-path problem examine local constraints in some 
neighborhood of real space or in C- Space, and propose motions based on the 
geometry of the neighborhood. Typically, local algorithms are implemented as 
searches, and the examination of constraints near a search node leads to the 
selection and application of some local operator to move the robot in space. For 
example, in our algorithm for the six degree of freedom Movers' problem, the local 
constraints correspond to the geometric structure of C-surfaces in a neighborhood, 
and local operators consist of motions along C-surfaces. 1 In general, a local planning 
algorithm will be complete if (1) the closure of the local operators is complete for the 
arcwise-connected components of C-Space, and (2) each local operator attempted 
ensures that a collision-free path exists between configurations in the search. 

However, our observation has been that in general, even complete local 
algorithms can get lost examining irrelevant local constraints. In particular, without 
adequate knowledge of the connectivity of a workspace and the classes of paths 
it contains, such methods may choose impossible or ill-advised candidate paths: 
hence they may take a long time to converge. 

A global find-path algorithm attempts to construct a model of the connectivity 
of the workspace. We believe that the connectivity of configuration space can be 
inferred from the connectivity of real space. Good hypotheses about the channels, 
or classes of paths through free-space can serve as guidance for a more detailed 
method. While there exist several proposals for global approaches to the Movers' 
problem, in Donald (1983a) we attempt to formalize criteria for the design of such 
algorithms. A global planner based on these criteria was implemented, and coupled 
with a complete local algorithm to form an integrated planning system. 

Channels are an encoding of free-space corresponding to the classes of paths 

within an environment. An implementation exploiting this global model of the 

connectivity of free-space has been able to solve two dimensional find-path problems 

in several minutes which formerly took many hours. The algorithm is essentially 

1 This example is illustrative and typical of the local constraints and operators. The implemented 
planner is more complicated, as we shall see. 
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a problem-solving strategy using a homcomorphic reduction of the search space. 
See Donald (1983a) for a description of the channel algorithm. In appendix III, 
we discuss the design and integration of local and global planning algorithms in 
3? 3 X 50(3). 

1.5. Review of Previous Work 

1.5.1. Introduction 

In this section we review previous work on geometric planning problems. 
We also give a formal characterization of completeness for the spatial planning 
problems. A survey of robotics issues in robot motion planning can be found 
in Brady, et al. (1980). For related work on the mover's problem, see Brooks 
(1983a), Lozano-Perez (1981, 1983), Lozano-Perez and Wesley (1979), Brooks and 
Lozano-Perez (1983), Schwartz and Sharir (1982a), Reif (1979), Moravec (1979), 
Udupa (1977) and Hopcroft and Wilfong (1984). Wingham (1977) and Popplestone, 
Ambler, and Bellos (1980) consider related issues in geometric planning problems. 
Some issues in automated structural design are addressed in Donald (1983b). For 
a review of geometric modeling techniques, see Baer, Eastman, et al. (1979) and 
Requicha (1980). 

1.5.2. Complexity-Theoretic Results 

In seminal work on the complexity of the Movers' problem, Reif (1979) has 
shown that the motion planning problem for a robot with an arbitrary number 
of degrees of freedom in the form of arm-like linkages is P-Space-hard. Hopcroft, 
Joseph, and Whitesides (1982) have shown similar results for planar manipulators 
with n linkages. In general it has been found that with n degrees of freedom, the 
problem is P- Space-hard. Happily, Schwartz and Sharir (1982a) have demonstrated 
the existence of a polynomial- time algorithm for the Movers' problem with fixed 
degrees of freedom, where the size of the problem is measured in the number of 
obstacle faces in the environment. The algorithm of Schwartz and Sharir (1982a) 
for the classical Movers' problem is unfortunately of time complexity 
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where n is polynomially dependent on the number of faces in the environment, and d 
is the number of degrees of freedom. For 6 degrees of freedom this becomes 0{n wm ). 
Hence it serves chiefly as an existence proof for a polynomial- time algorithm. 

The coordinated motion problem has also been given some attention: Schwartz 
and Sharir (1982b) address the problem for 2 and 3 circular bodies moving amidst 
polygonal obstacles in the plane. The coordinated motion system has degrees of 
freedom equal to the sum of the degrees of freedom of the moving bodies. These 
results lead us to expect exponential behavior from all motion-planning algorithms 
as the number of degrees of freedom grows. For these reasons we will confine 
ourselves to the classical Movers' problem, which has 6 degrees of freedom in 
3-dimensional space. 

Reif (1979) also sketches a polynomial- time algorithm for the classical Movers' 
problem, but it appears incomplete in that it ignores constraints arising from the 
interactions of faces of the moving object with vertices of obstacles, and does not 
consider edge-edge interactions in 3 dimensions. 

1.5.3. Work in Computational Geometry and Robotics 

The foundations of our approach lie in Lozano-Perez (1981, 1983), Lozano-Perez 
and Wesley (1979) and Schwartz and Sharir (1982a). The problem of moving a 
complex polyhedral object among obstacles is transformed to the problem of finding 
a path for a point in a high-dimensional configuration space. 

Brooks and Lozano-Perez (1983) have implemented a general path-finding 
algorithm for a polygonal object in the plane with two translational and one 
rotational degrees of freedom. Their planner uses hierarchical subdivision of the 
3-dimensional configuration space 5J 2 X S 1 . The subdivision algorithm has been 
specialized to the particular geometry of the Movers' problem in 3? 2 X S 1 and 
while in principle it is extensible to the 6 degree of freedom problem, its space- 
complexity in high dimensions is likely to be unattractive. A problem with the 
hierarchical subdivision strategy is that it has trouble exploiting coherence in 
C-Space. Its spatial taxonomy is restricted to filled, empty, and mixed, in a 
world where almost everything is mixed. Mixed cells are subdivided until an empty 
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region is found. However, it is hard to propagate this useful information to guide 
the search through nearby, unrefined cells in the subdivision. One goal of the 
algorithms and representations in this thesis has been to exploit coherence in the 
configuration space. The intuitive appeal is that the intersections of C-surfaces "go 
somewhere useful" (i.e., around the obstacles). We will adopt an approach which 
exploits the coherence of C- Space obstacles by moving along the intersections of 
high-dimensional C-manifolds 2 parallel to the obstacle boundaries. 

Lozano-Perez (1981) has described approximate solutions for Cartesian manip- 
ulators with 6 degrees of freedom (in principle) which consider 3-dimensional 
slice-projections of Configuration space. In practice these approximations are only 
reasonably accurate for Cartesian manipulators with 4 degrees of freedom. In 
principle, the C-Space constraints on motion defined by Lozano-Perez (1983) can 
be extended directly to a 6 degree of freedom planner; indeed, this is our starting 
point. However there are many interesting and complex problems to work out (see 
Brooks (1983b) for another discussion of these problems). In particular, there are 
many unresolved mathematical details for the 6 degree of freedom case. Given 
the mathematical model, there still remains the issue of a complete planner which 
exploits the model. 

1.5.4. Global Methods 

Global methods for path planning attempt to construct a model of the 
connectivity of free-space which can be related to the Voronoi diagram (see 
Drysdale (1983)). In particular, Brooks (1983a) has implemented a 2-dimensional 
path-planner which models the free-space as an overlapping union of Generalized 
Cones (Binford (1971)). Each cone provides orientation constraints on motion within 
the cone, and these constraints are intersected to find a translational path along the 
cone axes (called spines) interspersed with rotations at the spine intersections. This 
work was extended to a six-link manipulator for moving payloads with 4 degrees 
of freedom (Brooks (1983b)). The extended algorithm uses the same cone model, 
but sweeps each cone vertically to build prisms at horizontal slices through the 
workspace. This method works well when the payload (or polygon) is small and 



2 A C-manifold is a manifold in a configuration space. 
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convex in a relatively uncluttered obstacle environment. It is not at all clear how 
to extend the algorithm to large, non-convex moving objects, or how to consider 
more than one rotational degree of freedom at a time. Nevertheless the concept of 
computing "freeways," or "channels" through free-space is attractive in that it can 
provide global guidance to local algorithms (such as C-Space methods), and can 
enumerate good hypotheses about candidate paths through complex workspaces. 

Using an approach called retraction, 6'Diinlaing and Yap (1982), O'Dunlaing, 
Sharir and Yap (1982) construct a Voronoi diagram for a two-dimensional workspace 
and consider moving simple objects (a disc, a line-segment) along it. This technique 
was mentioned by Brooks (1983a). It has not yet been extended to polygonal objects 
or 3-dimensional cases. We will address this issue by considering Generalized 
Voronoi Manifolds. 

1.5.5. Approximation and Completeness 

Planning problems have two components: characterizing the constraints, and 
searching for a solution which satisfies the constraints. One attempts to achieve a 
complete (in some sense, "exact") characterization of the constraints, and a complete 
search algorithm for the representation. Since the Mover's problem is a continuous 
mathematical decision problem, we must in general consider a discretized version of 
the problem (see Reif (1979)), for example, we might represent the polyhedral input 
models as systems of linear inequalities within a fixed accuracy e, with < e < 1. 
In fact, there are two kinds of resolution limit. Any algorithm which employs real 
arithmetic has a resolution limited to the machine precision. (Schwartz and Sharir 
(1982a) employ rational and algebraic numbers instead). For the find-path problem, 
we are interested exclusively in the physically realizable paths, that is, those paths 
lying entirely within open sets of free space. The resolution limit Reif mentions is 
essentially a bound on how small an open set can become before it is no longer 
considered open. The open set resolution limit is typically greater than the machine 
precision. 

Almost all find-path search algorithms are complete only to this fixed resolution; 
the notable exception is Schwartz and Sharir (1982a), which appears to be 
search-complete and resolution independent. We should stress that for a complete 
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representation, the resolution-dependence is in practice not a severe restriction. 
However, the effect of a complete search algorithm running on an approximate 
characterization of the constraints is not clear. In principle, in case of search failure, 
it is sometimes possible to refine the approximation and redo the search until a 
path is found. This possibility has rarely been exploited however, and introduces a 
number of unpleasant technical and conceptual issues. A complete search running 
on an approximate representation will in general result in an incomplete algorithm. 
For these reasons we would prefer a complete characterization of the constraints 
coupled with a complete search algorithm. 

We will place this thesis in the context of previous work by considering the 
following criteria: 

(i) For what degrees of freedom does the algorithm apply? 
(ii) Is the representation (the characterization of the constraints) complete? 
(iii) Is the search complete (at a given resolution)? 
(iv) Has the algorithm been implemented? 

Approximate Representations 

Much of previous work has focused on approximate characterizations of the 
constraints. Approximate representations may (1) artificially restrict the degrees of 
freedom in a problem, (2) bound objects in real-space by simple objects such as 
spheres, or prisms with parallel axes, while considering some subset of the available 
degrees of freedom, (3) discretize configuration space at certain orientations, or (4) 
approximate swept volumes for objects over a range of orientations. Such restricted 
planning systems may lose solutions which require exploiting all six degrees of 
freedom. An approximation of the obstacle environment, robot model, or C-Space 
obstacles can result in a transformed find-path problem which has no solution. 

Some approximate algorithms-for example, those of Brooks-run quite fast 
for the class of problems that they address. In general, speed has been a 
motivating factor in the design of these approaches. We also observe that some 
approximate methods were motivated by the difficulties of modeling constraints in 
a full 6-dimensional C-Space. These difficulties in turn stemmed from unresolved 
mathematical problems relating to both C-Space itself and to the structure of 
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C-Space constraints. However, even with a, complete mathematical model in hand, 
we are still confronted with the problem of devising a complete planner which 
works using the full set of constraints. 

The configuration space of the three dimensional classical Movers' problem with 
six degrees of freedom is 3ft 3 X 50(3), where 50(3) denotes the three-dimensional 
rotation group. In this thesis we first complete the mathematical framework for the 
configuration space 3ft 3 X 50(3) and present solutions for some heretofore unsolved 
problems. This foundation then allows us to propose and construct a complete 
planner exploiting the full set of constraints and 6 degrees of freedom for motion 
planning in 3ft 3 X 50(3). 

In this section, we characterize the completeness of previous work. Unless noted, 
search-completeness is resolution-dependent. Schwartz and Sharir (1982a) describe 
complete representations and complete (unimplemented) search algorithms for 2D 
and 3D. These theoretical algorithms appear to be resolution-independent. Brooks 
and Lozano-Perez (1983) describe complete representations and search algorithms 
for the problem in 3ft 2 X 5 1 . Lozano-Perez (1981, 1983), Lozano-Perez and Wesley 
(1979) give approximate representations (except for translation) with complete 
search algorithms for 3ft 2 X 5 1 and 3ft 3 X 50(3). These approximate representations 
also model Cartesian manipulators. Most algorithms for 3ft 3 X 50(3) can be extended 
for Cartesian manipulators in a similar manner. For translations, Lozano-Perez' 
algorithms are complete to the machine resolution. Brooks (1983a) provides an 
approximate constraint characterization with a complete search algorithm for 
3ft 2 X 5 1 ; Brooks (1983b) extends this for a linked arm carrying a payload with 
degrees of freedom Si 3 X 5 1 . A significant contribution of Brooks was the addressing 
of the issue of jointed arms. The open set resolution limit for the Voronoi methods 
(for simple objects in two dimensions) is no larger than the machine precision. 
Udupa (1977) and Widdoes (197-1) used approximate representations and incomplete 
search algorithms in addressing find-path for jointed arms. 

In this light, we can characterize our algorithm as follows: 

• This thesis presents the first implemented, representation-complete, search- 
complete algorithm (at a given resolution) for the classical Movers' problem in 
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SR 3 X SO{3). 
1.6. An Outline of this Thesis: Research Contributions 

In this thesis we present a local algorithm for the six degree of freedom 
classical Movers' problem. The channel based algorithm developed in Donald (1984) 
is described in Donald (1983a). 

At the heart of this research lie certain mathematical developments that may 
seem fairly abstract at first reading. To motivate the mathematics, we first present, 
in chapter 2, the design and implementation of a six degree of freedom planning 
system for the classical Movers' problem. The description of the planning algorithm 
assumes that certain representations and mathematical tools are available. In 
subsequent chapters, we develop these tools in answer to the following questions, 
for which chapter 2 assumes solutions: 

Representational and Algorithmic Questions 



(i) What is an appropriate representation for constraints in a six dimensional 
C-Space? (Chapter 3). 

(ii) In the six dimensional C- Space of the classical Movers' problem, the domain of 
each constraint is the product space of 5ft 3 and a complicated three-dimensional 
manifold (with boundary) on the projective three-sphere. What are these regions, 
and what is their structure? What representation can be used for these domains? 
(Chapter 3). 

(iii) How do we plan motions using constraints whose domains change with the 
motions? (Chapter 5). 

(iv) Given a trajectory in C-Space, it is necessary to find where it intersects 
the boundary of C-Space obstacles. How can trajectories be intersected with 
C-surfaces whose domains change along the trajectory? (Chapter 4, 5). 

(v) How can intersection manifolds be constructed in C-Space? (Chapter 4). 

(vi) How are motions planned that slide along C-surfaces and intersection manifolds? 
(Chapter 4, 2). 

(vii) How can rotation space be decomposed into regions where the set of applicable 
constraints is invariant? (Chapter 5). 
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How to Read this Thesis 

Chapter 2 — covering the design and implementation of the search algorithm in 
C-Space — presents the most heuristic component of this research. It is also in some 
sense the most accessible chapter to the non-specialist. However, do not confuse 
chapter 2's implementation details and search heuristics with the representational 
and algorithmic framework developed under the considerably more formal asgis of 
chapters 3 through 6. The thesis is structured so that those prefering a presentation 
more in keeping with the traditional style of mathematical exposition may read the 
chapters on 6DOF planning in the alternative order: 

(1) Geometric Planning Problems 

(3) Questions of Representation: C-functions and Applicability Constraints in a 
Six Dimensional Configuration Space 

(4) Mathematical Tools for Motion Planning in a Six Dimensional Configuration 
Space 

(5) Moving Through Rotation Space 

(6) The C-Voronoi Diagram and its Relationship to Intersection Manifolds 

(2) A Planning System for the Classical Movers' Problem with Six Degrees of 
Freedom. 

In the alternative order, the representations and algorithms are derived and 
presented first, and the application and implementation is presented last. 

Chapter 3 presents a formal framework in which several open questions about 
configuration space constraints — notably (ii) (above) — may be solved. Chapter 3 
also derives fundamental structural properties of C-Space constraints, in particular, 
the domains and domain topology of C-functions for the classical Movers' problem. 
We call these domains applicability constraints. Chapter 4 addresses the intersection 
problem in high-dimensional C-Space: how to construct and slide along intersection 
manifolds, and how to intersect trajectories with C-surfaces and applicability 
constraints. We demonstrate the form of the intersection manifolds for 5R 3 X 50(3) 
and 5R 2 X S x . Chapter 5 discusses algorithms for moving through rotation space, 
and for decomposing rotation space into equivalence classes where the set of 
applicable constraints is invariant. In chapter 6, we extend the concept of the 
generalized Voronoi diagram (which Drysdale (1983) defined for the plane) to the six 
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dimensional C-Space 3R 3 X 50(3), to provide a formal and constructive definition 
of the C-Voronoi Diagram, or CVD. The CVD is an attractive construction, in that 
it contains a representative component for each "branch" of free space. Each such 
component is a submanifold of dimension < d < 5, called a Voronoi manifold. 
We will derive the following connection between intersection manifolds and the 
CVD: 

Theorem: (The Equivalence Theorem for intersection manifolds and the CVD). 
Let p be a path along the CVD. p lies along a connected chain of Voronoi 
manifolds, Vi, . . . , Vfc. We demonstrate that for each Voronoi manifold V{, there 
exists an equivalent intersection manifold of level C-surfaces, I{. Furthermore, 
we also show that for every connected chain of Voronoi manifolds, there is an 
equivalent connected chain of intersection manifolds (of level C-surfaces). (The 
equivalence we demonstrate is actually stronger than homotopic equivalence, 
but the additional details are too complicated for this chapter). 



67 



2 
A Planning System for the Classical 
Movers' Problem with Six Degrees of Freedom 

In this chapter, we describe the design and implementation of a planning 
system for the classical Movers' problem with six degrees of freedom. The planning 
algorithm required the solution of the seven "Representational and Algorithmic 
Questions" listed at the end of chapter 1. The solutions to these problems are 
presented in subsequent chapters. 

In this chapter we will simply assume that these problems are solved, and 
proceed to employ the solutions in constructing a planning algorithm. Of particular 
importance will be two effective procedures, which address the intersection problem 
in C- Space: 

(I) Given two or more level C-surfaces, construct their intersection manifold. 
(Chapters 3 and 4). 

(II) Given a C-surface and a trajectory, find their intersection. Determine 
whether the intersection lies on the boundary of a C- Space obstacle. (Chapters 
4 and 5). 

The immediate application of (I) is the sliding problem: How to slide along 
one level C-surface, and how to slide along the intersection of two or more level 
C-surfaces. 
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Using the point navigation operators (chapter 1), we implemented a best-first 
search algorithm in C- Space. The algorithm has nice theoretical properties which 
include completeness (at a resolution). This chapter describes the heuristic search, 
with particular emphasis on the heuristic strategies that evaluate local geometric 
information, and on the interaction of these strategies. 

2.1. Definitions 

A topological space M is called an n-dimensional manifold if it is locally 
homeomorphic to 9i n . A chart is a way of placing a coordinate system on M: if 
U and V are open subsets of M, two homeomorphisms f : U —> f(U) C 3ft n and 
g : V -+ g(V) C 3R n have C°° overlap if the maps 

/ o g- 1 : g{U f]V) - f(U f]V) 
9 o r' : f(U C[V) -, g(U (]V) 

are also C°° (that is, possessing continuous partial derivatives of all orders). A 
family of pairwise C°°-ovcrlapping homeomorphisms whose domain covers M is 
called an atlas for M. A particular member (/, U) of an atlas U is called a chart (for 
the atlas U), or a coordinate system for U. For a good introduction to differential 
geometry, see, for example, (Spivak, 1979). 

In this thesis we usually specify charts via the inverse form h : R ->• M 
(where R is an open subset of 3£") with the understanding that it is the inverse 
(or set of local inverses) h~ l which provides the family of charts {(h~ l ,Wi)} f for 
[JiWi — h(R). As an example, consider the map h that specifies a chart for a five 
dimensional level C-surface: 

h : 3fc 5 -> SR 3 X 50(3) 
(y, z, if,, 0, <j>) h+ ( — ,y, z, V>, e, 4>). 

Here the E{ are smooth, real-valued functions on 50(3), that is, E{ : {ip,0,4>) -> 3i. 
The inverse map h~ x is obvious, and provides a chart for the five dimensional 
submanifold of 3tf ;! X 50(3). In subsequent chapters we will derive such charts, in 
the form of h; in this chapter, we will take them for granted. 
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2.2. Introduction 

We are now ready to describe a planning system for the find-path problem in 
9ft 3 X 50(3). The algorithm has the structure of a search and is complete (for a 
given resolution). The basic idea is as follows: we are able to define and implement 
certain local operators. When applied at a configuration in C-Space, a local operator 
attempts to move the robot in a specified direction until either the subgoal or an 
intervening C-surface is reached. The local operators have the general form 

Move(X:configuration,v:direction,limit:con figuration), 

and are designed to return X', the configuration reached in direction v, and 
the reason for stopping (which will either be "reached subgoal" or the name of 
the C-surface which halted progress). The local operator assumes that X is in 
free-space, and ensures that there exists a collision-free path along v taking the 
robot from configuration X to X'. Furthermore, we insist that limit = X + tv, for 
some positive t. In general, v can be represented as a tangent vector to 9ft' X SO(3); 
the space of directions is clearly locally homeomorphic to 9ft . 

Many different Move operators can be defined. Let X = (x, 9). We will restrict 
v to be either a pure translation 

v <= 3ft 3 x { e } 

or a pure rotation 

The closure of these operators is complete for the space of configurations. By 
this we mean that in the absence of obstacles, there is some finite sequence of 
operators which carries any configuration X into any other configuration Y. It is 
often convenient to think of these operators as Translate(X,u,x') (where u G 3ft" 
and x' is a goal translation) and Rotate(X,<p,<p') (where <p is an angular direction 
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and <p' is a goal angle). The theory and implementation of Translate and Rotate 
is discussed in chapters 3, 4, and 5. 

Given the local operators, we can define more sophisticated local strategies for 
spatial reasoning. These strategies are implemented by local experts 1 in C-Space. 
For example, one local expert attempts to circumnavigate C-Space obstacles by 
sliding along intersections of level C-surfaces. Another, "greedy" expert tries to 
translate or rotate straight towards the goal. A local expert typically examines the 
local geometric environment of C-surfaces, their normals and intersections. It also 
takes into account the history of planning. The local experts can be thought of as 
issuing "commands" in terms of the local operators. Depending on the results of 
these attempted motions, an expert may issue other local operator commands, and 
either directly invoke or leave a forwarding message for another local expert. 

To summarize: a local operator is an algorithm for moving along a specific 
trajectory until a constraint is encountered (or a subgoal is reached) . A local expert 
is a strategy for choosing the trajectory based on an examination of the history of 
planning and the local geometry. When a local expert chooses a trajectory, it calls 
on some sequence of local operators to realize it. 



'The term local expert was brought, to my attention in discussions with Van-Due Nguyen (Nguyen 
(1083)), Tomas Lo/ano-l'erez, and Rodney Drooks. 
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Figure 2.1. Schematic illustration of the "Bumble" strategy j[an exhaustive search). A fine 
six-dimensional lattice is thrown across C-Space. By exploring from one configuration to its 
neighbors in the lattice, a path will eventually be found, if one exists at the lattice resolution. 
Fortunately, it is also possible to take large steps in the lattice, and simply record the neighborhoods 
the path visits. ___^__ 

2.2.1. Planning and Search 

The planning algorithm is implemented as a search of configuration space. The 
search constructs a graph of neighborhoods which have been explored. (We will be 
more precise about the term neighborhood later). Each node in the search graph is 
associated with a configuration and contains information about the local geometry 
and the history of planning. The search algorithm chooses a node for exploration. 
Several local experts are then applied at that node. Each expert can produce a new 
search node. All of these are sons of the explored node, and are added to the search 
queue. The new sons are connected to their father by the arcs of the search graph 
and each son may be thought of as an exploration from the father. 

If at any point in the search, two explorations reach the same neighborhood, 
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the planner attempts to merge the associated nodes into one node. 

The search algorithm is Best-First (Nilsson (1980)) with the metric of progress 
established as distance from the goal. (This requires placing a metric on both 
translation and rotation space). Other search measures (such as path length, or 
time) would also be possible, and an A* search strategy could be exploited to find 
optimal paths. In practice this would probably require adding new local experts in 
order to ensure reasonable performance. 

As search nodes are explored, they are entered in a priority queue, called the 
search queue. The nodes in the search queue are ordered by the search metric. 
Some search strategies we discuss require two search queues: when the primary 
queue is exhausted, then nodes from the reserve queue are explored. 

We will proceed as follows. First, using the local operators alone, we can define 
a complete search strategy (at a given resolution). This search strategy can be 
considered the most primitive local expert, and is known as the "Bumble Strategy." 
By applying the Bumble strategy at every search node, we are guaranteed to find 
a path (at a given resolution) if one exists. 

Next, we will define more complicated local experts which will be applied to 
search nodes at the same time as the Bumble expert. These experts greatly improve 
the performance of the planner. 



73 



2.3. A Complete Search Strategy 

A search node is associated with a configuration. Every configuration is in turn 
associated with a neighborhood of C-Space. The neighborhoods form a partition 
of C-Space. Since many configurations are associated with one neighborhood, so 
several search nodes may have configurations lying in the same neighborhood. 

Assume the neighborhoods are "small." If the configurations of two search 
nodes are in the same neighborhood, it indicates that they should, if possible, be 
merged into one node, since they are close together. By keeping track of the set of 
explored neighborhoods, we can avoid redundant explorations. If the neighborhoods 
are sufficiently small, then the search will be complete at a resolution closely related 
to the neighborhood size. 
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Figure 2.2. /, g : !R 3 X S0(3) -» 3? are C-functions which describe two level C-surfaces, ker / 
and kerg. The level C-surfaces are smooth, 5-dimensional manifolds parallel to C-Space obstacle 
boundaries. From X G kcr /, three paths sliding along the level C-surface ker / are shown. 
Each path is orthogonal to V/. The sliding expert plans such paths along 5-D level C-surfaces. 
(ker /) fl(kcr g) is the intersection of the two level C-surfaccs, and is a 4-dimensional manifold. 
The intersection expert plans paths along intersection manifolds. Such a path p is shown from 
configuration Y. ^___ 

It is possible to devise a complete search strategy (at a given resolution) using just 

the local operators. We first throw a fine six-dimensional lattice 2 over configuration 

space. The lattice is used to keep track of the state of the planner, i.e., which 

neighborhoods have been explored, and for computing the connectivity of these 

neighborhoods. The lattice will "wrap around" in the rotational dimensions, but 

this is easily implemented using modular arithmetic. We will define an adjacency 

function for points in the lattice; in addition, when a neighborhood is explored, 

the corresponding node in the lattice is marked. When a search node is chosen for 

exploration, 



2 l.c., the factor spaces of the parameter space are quantized, and the lattice is a partial order 
on the Cartesian product of the factor space quantizations. 
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(i) X, the configuration of the search node is mapped to L, a point in the lattice. 
L is the name of the neighborhood M(L) centered on L, which contains X. 

(ii) The unexplored neighborhoods adjacent to M(L) are found. Each of these 
neighborhoods is also identified by a central lattice point. 

(iii) The planner attempts to move to each of the unexplored, adjacent neighbor- 
hoods. 

(i) has the effect of mapping a neighborhood of C- Space to a canonical element 
(which lies on the lattice) in its interior. These neighborhoods decompose 5ft' J X SO(3) 
into equivalence classes with the same canonical element. When a neighborhood 
is reached for the first time, we mark its lattice point as explored. The search 
terminates when a neighborhood containing the goal is reached, and when that 
exploration can be connected to the goal configuration. 
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2.3.1. Implementation of Neighborhoods and Lattices 

In principle, it is possible to implement the lattice as a six-dimensional array 
(with modular indexing for the rotational dimensions). In practice, for any fine 
resolution, this array will be enormous, and very sparse. Although an adversary 
can design a find-path problem for which our planner must explore the entire 
lattice, in practice this does not occur. However, we must maintain a record of what 
neighborhoods have been explored, in order to generate the unexplored neighbors 
for a search node. Since the array is sparse, we will employ a different strategy. 

A partial order can be defined on lattice points by considering them as 
six-dimensional vectors. This order has no particular geometric significance for 
the rotational dimensions, but it can be used to store explored lattice points in a 
binary tree. Since the vast majority of neighborhoods are never explored, the tree 
it typically small, even for fine lattices. To mark a lattice point as explored, we 
insert into the binary tree. To find whether a lattice point has been explored, we 
search the tree. 

It is desirable to employ a fine lattice in order to ensure completeness at a fine 
resolution. The use of a binary tree to record explored configurations effectively 
removes the problem of lattice size for storing explored configurations. For example, 
if wc segment C-Space into an N X N X . . . X N lattice, then an array would 
have to be N 6 long. But the binary tree need store only the explored locations, and 
(if height- balanced) can access any leaf in O(logiV) operations. 
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If the lattice resolution is fine, then the planner as described so far will take 
very small steps for each search exploration. This has been remedied as follows: 
If a local operator is invoked to find whether limit may be attained from X in 
direction v, it must effectively intersect a path in direction v with all C-surfaces. It 
is not much harder to find the first constraint along the path p(t) — X + tv (even 
if it is beyond limit): in particular, we note that all intersections along the path p 
may be sorted on distance from X. The complexity of finding this first intersection 
along p is independent of the lattice resolution (since the intersection algorithm has 
nothing do do with the lattice; see chapter 5). We can "sample" the portion of the 
path which lies in free space at the lattice resolution. All of these configurations 
are then marked as "explored", and as reachable from their immediate neighbors 
along the path. Thus they form a connected chain in the lattice along the path p. 
While all these configurations are in some sense sons of X, in practice we will select 
only one or two to be entered in the primary search queue. These sons might be 
(1) the son which is closest to the goal, and (2) some son at a reasonably large step 
away from X. This step size, called the Bumble resolution, might be 3 to 10 times 
the lattice resolution. The other sons should be kept on a reserve queue, which can 
be explored when the primary search queue is depleted or exhausted. 

In practice, it may preferable to enter ranges in the exploration tree, for 
example, to record that all lattice points 

(x, y, z, ip,0, (j>) < L < (x + kd T , y, z, i/>, 0, <£) 

(for some integer k) are explored. This requires keeping an exploration tree of lines 
instead of configurations, with the intent of minimizing the number of exploration 
tree entries. When lines are entered into the tree, they may be merged with previous 
lines to form connected components of explored regions. These operations are 
supported by hierarchical subdivision algorithms. At this point in the experimental 
use of the planner, it is still too early to tell whether this optimization is necessary. 
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In practice we have had no problem in selecting a very fine resolution for 
the lattice (one selects a fine lattice resolution, and a considerably larger Bumble 
resolution or step size, as described above). This lattice-based strategy is not 
only theoretically complete for a given resolution, but has also been used to find 
very complicated paths for the 6 degree of freedom classical Mover's problem. 
However, the algorithm has an "excessively local" flavor — it is clumsy and quite 
slow when employed alone (hence the strategy's name). We can construct much 
"smarter" heuristic experts which attempt to exploit coherence in C-Space. When 
these experts are used in conjunction with the Bumble strategy, we obtain a 
planner which is not only complete, but which can solve complicated problems in 
a reasonable amount of time. We continue to find the lattice useful for recording 
the planner's explorations by the local experts. 

2.3.2. Keeping Track of Connectivity 

Suppose a subsequent exploration reaches the same neighborhood. There are 
two choices, which we call the mark algorithm and the connect algorithm: 

The Mark Algorithm. Discard the exploration, since the neighborhood is already 
explored. In practice, the mark algorithm often suffices for path-finding. The mark 
algorithm computes a directed, spanning tree T of explored neighborhoods, which 
is rooted at the start configuration. 

The Connect Algorithm. Connect together the search nodes for all explorations to 
that neighborhood. The connect algorithm is more complicated, and requires the 
following bookkeeping (see figure 2.3). Let M be a neighborhood of 8ft 3 X 50(3), 
and L £ i(M) be a lattice point which is the canonical element for M. Suppose X is 
an exploration of M, i.e., X £ H is the final configuration in some motion reaching 
M. Let s(X) denote the search node for X. (If X is the first exploration of M, then 
create a search node s(L) for L). Determine whether there exists a path from X to 
L (using the local operators). If so, connect s(X) and s(L) together. 

The connect algorithm computes a more complete connectivity graph for 
the neighborhoods of C-Space. It computes an undirected graph H of explored 
neighborhoods, which may contain cycles. As long as H is connected, then T 
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Figure 2.3. The lattice point L is at the center of a neighborhood M of C-Space. Search 
explorations arrive at configurations X and Y in M. The planner attempts to find a path 
connecting X and Y, by trying to connect both configurations to L. 

is a spanning tree for H, and the mark algorithm is complete for planning a 
connected path along H. However, not all planning strategies admit this kind of 
"connected planning." In particular, when we consider strategies which construct 
partial paths and planning islands (which may later connect up), the connect 
algorithm is necessary. (See the Suggestor strategy, below, for an example). 



2.3.3. Discussion of the Bumble Strategy 

Suppose the lattice spacing is d T and d R in the translational and rotational 
dimensions. Then the adjacent lattice points to L — (x,y,z,ip,9, <f>) will be: 
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(x±d T ,y,z,ip,0,(j)) 

(x,y±d T ,z,^,0,(f>) 

(x,y,z±d T ,ip,0,<t>) 

(x, y, z,tp± da (mod 2tt), 0, <f>) 

(x, y, z,ip,Q ± d/t (mod 2tt), <f) 

(x, y, z, if>,0,4>± dn (mod 2n)) 

Each adjacent lattice point is the center of a neighborhood of configurations which 
is contiguous to the neighborhood of L. Each such neighborhood can be reached 
(if it is in free space and there is no intervening C-surface) by the local operators 
Translate and Rotate. Since there are 12 neighbors for each lattice point, we have 
found it inadvisable to explore them all for each search node expansion. Instead, 
the set of unexplored adjacent neighborhoods is ranked (in terms of proximity 
to the goal), and motions towards the top fcj- translational and ka rotational 
neighbors are attempted. (Typically, k T p» 3 and k R p=s 2). If the node is recxplored 
later, motions toward kr + k^ more of the unexplored neighbors will be attempted 
(if there are that many left). When using the mark algorithm (above), we say 
an exploration is successful if it reaches a new (unexplored) neighborhood. If an 
exploration is successful, then a new search node is created and the neighborhood 
is marked as explored. Since the neighborhood's "name" is its lattice point, this 
simply corresponds to marking the lattice point. Whether successful or not, all 
explorations are recorded at the parent search node so that they will not be tried 
again. 

Suppose X is a configuration in neighborhood M(L), with associated lattice 
point L. The unexplored adjacent lattice points to L indicate a set of subgoals to 
be attained from X. The Bumble strategy ranks these subgoals, chooses some of 
them, and selects trajectories which may attain them. The local operators are then 
employed to (try to) realize the selected trajectories. These explorations are then 
recorded so that only new explorations will be pursued in the future. Note that 
the planner is not constrained to move along the lattice, and that although the 
subgoals lie on the lattice, the motion from X to any subgoal does not, unless 
X = L. 

The local experts are considerably more sophisticated than the Bumble strategy. 
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Their subgoals need not lie on the lattice, and the motions specified to the local 
operators need not lie along the lattice. The lattice is still employed to keep track 
of the planning history and the connectivity of explored neighborhoods. 

Clearly, the arcwise-connected sets of lattice points are closed under the 
operators Translate and Rotate. If a path exists at the lattice resolution, then 
the search is guaranteed to find it. We see now exactly what the resolution for 
this find-path algorithm is: by choosing a sufficiently fine lattice, the algorithm is 
(trivially) complete at the lattice resolution. As we saw above, we can choose a very 
fine lattice with little computational overhead. One final point: the start and goal 
configurations may not lie directly on the lattice. This is not a problem, however, 
since the local operators can ensure that there exists a path from the start and goal 
to the nearest lattice point. 

2.4. Local Experts for the Find-Path Problem 

2.4.1. Path Planning versus Continuous Intersection Detection: Why We 
Need Local Experts 

The Translate and Rotate operators detect collisions along continuous 
trajectories. 3 Civen these operators, it is possible to devise a complete path-planning 
algorithm based on something like the Bumble strategy, above. However, while 
complete, this is not a particularly good algorithm, in that it says nothing about 
how or when the operators should be applied. The domain of the operators is 
large and for realistic path planning, it is necessary to know where, and in what 
directions to apply them. 

Algorithms which can detect intersections with obstacles for a robot following 
a continuous trajectory say nothing about how to plan these trajectories. 
However, they can be used to find a path by exhaustive search. 

The Translate and Rotate operators use the constraints in C-Space to detect 
collisions. However, these constraints can also be employed to plan paths. In 
chapter 1, we proposed an idealized planner which constructed the intersection 
3 This discussion also holds for the general Move operator. 
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manifolds of level C-surfaces, and slid along these manifolds to navigate around 
C-Space obstacles. Such a planner could exploit coherence in configuration space: 
by examining C-Space constraints an algorithm can be devised for intersecting 
and sliding on C-surfaces to circumnavigate C-Space obstacles. In the following 
sections, we describe a planner which approaches the idealized planning algorithm 
of chapter 1. The local experts are strategies for reasoning about the local geometry 
of configuration space, and for exploiting geometric constraints to plan collision-free 
paths. When applied to a search node, each local expert examines the local geometry 
and history of planning to propose one or more path segments. Each path segment 
is realized by means of the local operators, which ensure that a collision free path 
exists. 

2.4.2. Designing Local Experts 

In the exploration tree of C-Space neighborhoods, we have seen one type of 
information that must be maintained for planning. In designing local experts, we 
must address the following questions: 

(i) What constitutes a local description of a (level) C-surface? 
(ii) What information should be stored at a search node? 

(i) can be stated, "What constitutes a sufficiently rich description of the 
local geometry in C-Space to allow robust local experts?" (ii) relates more to the 
history of planning, and the connectivity of the explored search neighborhoods. 
For example, we want to record the results of previous applications of experts at a 
search node, and the adjacent nodes in the search graph. 

The Local Description of a C-surface 

A C-surface has a normal at point X. Motions tangent to the C-surface at X 
will have instantaneous velocities orthogonal to the normal. We must characterize 
the normal and tangents to a C-surface in order to plan trajectories which slide 
along it. 

Let / be an applicable, positive- valued C-function at X. We can check that / is 
non-redundant at X (see chapter 6); alternatively, we may heuristically assume / is 
non- redundant if its value at X is small. We wish to develop a local characterization 
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of / at X, that is, of the level C-surface S = {Y \ f{Y) = f{X)} about X. We 
should think of S as the kernel of the auxiliary function 

fx : SR 3 X 50(3) -> » 

Y -> f(Y) - f(X). 

The local characterization will have two parts, one of which is invariant, and one 
of which will change for different subgoals. The invariant part of the description is 
a pair, 



(f(X), V/) 



consisting of the value of / at X and the normal to S at X. Now, since 9J 3 X SO(3) 
is not a vector space, the normal V/(X) to S at X will depend on the Riemannian 
metric defined on the tangent space at X . We will employ a metric which admits 
construction of V/(X) using the partial derivatives of / at X , with respect to the 
parameterization of C- Space. Hence if rotations are parameterized by Euler angles, 
tnen vj — {^, ^, in> ■$$, w , -^). 

Assume that V/ is normalized to be a unit vector. We now wish to characterize 
the relationship of the C-surface to some siibgoal, G: this requires some way of 
talking about directions in 9?'* X SO(3). Specifically, we wish define a "vector" 
algebra on configurations, such that 

lim IIG -XII =0 
G~*X " " 

and 

J\m a (G-X).(G'-X) = l. 

These equations express the vector space characteristics which are required for 
our computations on tangent vectors. To construct this algebra, it is possible to 
define a field of inner products over 9?'* X SO(3), i.e., to define an inner product on 
the tangent space to each point. Thus 3? 3 X SO(3) is a Riemannian manifold (see 
Erdmann (1984)). If two tangent vectors-i.e., directions-are applied to the same 
point, this inner product allows us to talk about the angle between two such tangent 
vectors, or of the angle between an arbitrary tangent vector to 9J ,J X SO (3) and the 
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normal to a C-surface. However, the inner product is somewhat arbitrary for our 
application. Alternatively, we could also construct geodesies on P' , the 3-sphere 
with antipodal points identified. These approaches are probably too elaborate for 
a heuristic strategy. 

Heuristics for Evaluating Directions in the Tangent Space 

A basic issue is that placing a metric on a non-abelian group, such as 50(3), 
is a difficult problem. We will demonstrate the metric that our planner employs, 
and then show that it is adequate for this application. In particular, the metric 
is adequate when applied to three one-dimensional slices of 50(3). (These are the 
slices considered by the Rotate operator). Note, however, that a metric may also 
be derived by representing rotations as unit quaternions. In this case, the metric is 
obtained by considering rotations as points on 5 3 embedded in 9? (Brou 1983). 

Suppose we employ rotation matrices to represent rotations. (The implemented 
planner uses Euler angles). If we are willing to tolerate singularities in the 
representation, it is often convenient to identify a rotation matrix in 50(3) with 
the vector of three angles, (ip, 9, <j>) which determine it. The angles (ip, 0, 4>) form a 
three dimensional angle space, Q 3 . The rotation matrix corresponding to {xj),0,4>) is 
of course R.(ip,9,<f>). (The singularities induce an equivalence relation on Q 3 , where 
two points in angle space are equal when the rotation matrices they determine 
are equal). Most of the time, the identiDcation of 50(3) with Q 3 does not lead 
to problems. However, when we wish to compute directions, and differences of 
configurations, it is necessary to distinguish between 50(3) and Q 3 . 

We can state this more concisely as follows: 50(3) is a three dimensional 
manifold. The mapping k from Euler angles to rotation matrices is a chart for 
50(3): 

Z : Q 3 -> 50(3). 

We typically describe a rotation R(0) £ 50(3) by its chart coordinates (ip, 0, <j>) = 
© 6 Q 3 . This makes it convenient to identify © with £(©), so that in general, 
instead of dealing with the manifold directly, we will work with a chart for the 
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manifold. In this section alone, however, we must distinguish between the domain 
and image of JZ. 

We can compute a direction in 3? 3 X SO(3) by simply subtracting two 
configurations (of course the angles must be subtracted (mod 2tt)) to yield a 
six-dimensional direction vector. Using this arithmetic, the goal direction is denoted 
G — X. We will use the convention that the first three coordinates of G — X arise 
from 3? , and the second three coordinates arise from Q . 

Let G = (G x ,Gq) and X = (X x ,Xq). Since G — X is clearly well defined 
when G and X differ only by a translation, assume that G and X differ only by a 
rotation. Assume further that rotations are represented by Euler angles. Note that, 
in general G — X is not a rotation which carries the moving object at orientation 
G into the moving object at orientation X. However, G — X does represent the 
difference in orientation, i.e., it specifies a displacement in angle space which will 
carry G into X. For example, if Gq = (45°, 50°, 90°) and X = (45°, 45°, 45°) then 
there are rotation matrices JZ(Gq) and R(Xq) corresponding to each of Gq and 
Xq. (We use degrees, not radians in this example, since the symbol n will soon be 
used for a projection map). Note that 

£(45°, 50°, 90°) ^ £(45°, 45°, 45°)£(0°, 5°, 45°), 
where RJZ' indicates composition of rotations. However, the path in angle space 

p(t) = X & +t(G Q -X Q ) 

= (45°,45 o ,45 o ) + *(0 o ,5°,45 o ) 

(for t £ [0, 1]) will work, since it corresponds to the rotational path 

Z(p(t)) = R(X e + t(G e -X ( ,)) 

= £((45°, 45°, 45°) + i(0°, 5°, 45°)). 

Considering configuration space as the product space of the translation space 
and the angle space, we see that G — X is well defined. Ge — Xe specifies a 
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direction and a distance to be traveled in angle space in order to carry X© into G®. 
Furthermore, along the path from X© to G®, the corresponding rotations specified 
by the angle space trajectory p are well defined. For all G £ 3ft X 50(3), we will 
treat the space of directions G — X as the tangent space Tx to 3ft 3 X 50(3) at X. 
Properly, Tx is the product space of the tangent space to 3ft 3 at X x , and the three 
dimensional angle space Q 3 . 

We now define a map from Tx X Tx to the plane, which will function in place 
of an inner product. First, define the natural projection maps from Tx onto its 
factor spaces: 

^jrs : Tx -*■ 3ft 
(G-X)^(G X -X X ) 

7T0 : T X -> Q 3 
(G-X)^(G e -X Q ). 

Let u ■ v denote the standard inner product on D? 3 , for vectors u and v. If u 
and v are projections (under 7^3) of direction vectors in Tx, we say that u and v 
are translationally orthogonal if u • v = 0. Let (gi> <72> Q'i)t { w l> w 'l, ^3) € Q • Assume 
the each pair of angles qi and W{ (for i = 1, 2, 3) is normalized so that 

\qi-w { \ < 180°. 
(Note that this normalization is critical). Now, define 

nQ\{qi,q2,qii),{wi,W2,W3)j = q\w\ + qiw 2 + q^W3- 

nq will function in place of an inner product on Q*. We say that two rotational 
directions q and w are rotationally orthogonal if rxQ[q,w) = 0. 

We may now define <&x > which will function in place of an inner product on 
T x . First, let 

D = G-X 
D' ^G'-X. 
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Assume that D x , D' x , Dq, and Dq are all normalized to be length 1 (where the 
length of Dq is defined as hq{Dq, Dq)*). Finally, 

(p, £>') >-> L, 3 (.D) • n^(D'), n Q (n e {D), 7r e (D'))\ 

So $x yields a pair consisting of the dot product of the translational components 
of the direction vectors, and the nq product of the rotational direction vectors. If 
$x{D, D') — (0, 0), we say that D and D' are orthogonal directions in the tangent 
space Tx- Note that two directions are orthogonal if, and only if, their translational 
components are orthogonal and their rotational components are orthogonal. 

This discussion extends naturally to other representations for rotations. For 
example, if spherical angles (Kane and Levinson (1978)) are used, then the difference 
in orientation is the rotation carrying G into X, that is, Gq — Xq is a rotation 
carrying the moving object at orientation Xq into the moving object at orientation 
Gq. We should stress that the natural Riemannian inner product (Erdmann (1984)) 
could be used instead of 4>x- This would complicate the representations employed 
in subsequent chapters. $x an d nq are heuristic measures on directions in Tx- We 
will later discuss why, for our purposes, they are good heuristic measures. 

Evaluating Normals and Gradients to C-Surfaces 

The local description of a C-surface relative to some subgoal is designed to 
address the following qualitative questions: 

(i) Is the C-surface locally tangent or locally orthogonal to the goal direction? 
(ii) Is the C-surface locally orthogonal to any rotational motion? 

Recall that a level C-surface ker / is described by a real- valued C-function /. 
Assume that normals and tangent vectors are appropriately normalized. Question 
(i) may be resolved by examining 

*x{{G-X),Vf{X)). (2.1) 

When (2.1) approaches (0, 0), we say that ker / is locally tangent to the goal direction. 
Note that (2.1) makes sense: / maps parameters of the form {x,y,z,if},0,(j>) to real 
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numbers, and hence the gradient of /, 

fdj d£ df_ df_ dj_ dj\ 
\dx' dy' dz' dtp' 30' d<f>J 

is clearly a direction in Tx- 

We will also employ 

* R ,(G-JO.* s ,(V/pO), (2.2a) 

When (2.2a) approaches 0, we say that ker/ is (locally) translationally tangent 
to the goal direction. Symmetrically, when (2.1) (resp. (2.2a)) approaches (1,1) 
(resp. 1), we say that ker/ is locally orthogonal (resp. translationally orthogonal) 
to G — X. A similar calculation yields the rotationally tangent and orthogonal 
C-surfaces to the goal direction: 

n Q (n e (G-X), 7r e (V/(X))). (2.26) 

Why $x an d nq arc Good Heuristic Measures 

Suppose that the rotational direction is along one of the axes. (Let us say the 
direction is <j)). To tell whether a C-surface is rotationally orthogonal (or tangent) 
to the direction, we simply examine the magnitude of -d, which can be obtained 
directly from V/(X). This is because 

n Q (l ir e (V(/(X)))) = n Q ((0,0, 1), (||, f Q , g)) = |^. 

In other words, the map uq need not be employed. Since the implemented Rotate 
operator moves along the rotational axes in directions 

v e {+$,-$, +o,-o, +]>, -fry, 

this is the most common — but not the only — test for rotationally orthogonal (or 
tangent) C-surfaces. This information is used by the rotation experts to choose 
rotational subgoals that move away from C-surfaces. 
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Description of a Search Node 

The following information is stored at each search node. Lazy evaluation is 

implemented so that some of these objects (for example, the set of all applicable 

C-surfaces) may not be computed until they are required. 

i) The configuration X of the search node. 

ii) The lattice point for X , which is the unique identifier for the neighborhood 
about X. 

iii) The applicability set at X. 

iv) A, the set of non-redundant constraints at X , sorted on increasing value. The 
non-redundant constraints may be approximated by the applicable constraints 
having small positive (or zero) values at X. 

v) The parent node. 

vi) The From-Direction (The direction traversed from the parent node to this 
node). 

vii) The sons of this node. These include "unsuccessful" explorations which did 
not reach a sub-goal, or which reached a previously explored neighborhood. 

viii) The C-surfaces on which X lies which also bound C-Space obstacles, that is, 
all / £ A such that f(X) — and ker(/) bounds a C-Space obstacle at X. 

ix) An Explanation of how this node was reached. An explanation typically includes 
the name of the local expert that planned the move, and enough information 
to reconstruct the move. For example, the experts which slide along level 
C-surfaces leave an exphination containing the names of the constraints, their 
levels at the parent node, and the parameterization chosen for the intersection 
manifold. 

Much of the information stored at a search node is used to record the history 
of the planning. An expert which planned the move to a search node s will not be 
applied again with the same parameters. As an example, consider the Intersection 
expert, which attempts to slide along intersection manifolds, and the Greedy expert, 
which attempts to move straight towards the goal. We discuss these experts in 
more detail in the next section. If applied to s, the C-surface intersection expert 
will not attempt to construct and slide along the same intersection manifold 
which led to s, unless it can slide in a different direction along the intersection 
manifold. By recording the From-Direction for a node, the planner can avoid 
repeating unfruitful explorations. In particular, different experts can advise motion 
in the same direction; thus a particular intersection manifold may point in the 



90 



same direction which was previously (or simultaneously) attempted by the Greedy 
expert. Whether successful or not, reexploration in this direction may be avoided by 
examining the From-directions of the sons of s. An additional constraint is provided 
by the From-Direction of s itself: there is typically no point in exploring back in 
the direction we came from. The process of leaving information for some expert 
which may be applied in the future is known as "forwarding." As we shall see, the 
performance of one expert can provide strong hints as to what expert should be 
applied next. 

The planner computes local descriptions for the C-surfaces in A. Naturally, parts 
of these descriptions will change for different subgoals. The local characterizations of 
C-surfaces allow the planner to find the set of C-surfaces to which the goal- direction 
is tangent (or orthogonal) as described above. When a planning direction is chosen, 
these C-surfaces clearly provide strong constraints. 

We are now ready to discuss the experts themselves. The Bumble strategy is 
also applied at each node, since it is a guarantee of completeness. In light of the 
previous discussion, we will omit any discussion of the detection and pruning out 
of explorations in unfruitful directions (as determined by the planning history). 
We will consider the application of particular experts to a search node s (at 
configuration X) which has parent sq. 
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2.4.3. The Greedy Expert 

The greedy expert attempts to translate or rotate directly towards the goal. 
The expert is necessary as an "end-game" strategy, in order to close in on a 
particular subgoal without worrying about finding the appropriate intersection 
manifold. The Greedy expert illustrates two important heuristics: forwarding and 
backing off. Suppose the greedy expert translates from a parent node so to a 
son s. An appropriate explanation for the move will be left at s. If the same 
subgoal is intact when the planner explores s, the greedy expert will not attempt 
translation again. Instead, the rotation expert (see below) might be invoked. The 
effect is one of translating until an obstacle is hit, and then rotating to get around 
it. Alternatively, the sliding expert (which slides along level C-surfaces) might 
be invoked. This coupling of experts is termed the "hit and slide" strategy (see 
figure 2.4). However, the planner does not directly recurse by calling the sliding 
expert immediately after the greedy expert. Instead, a suggestion is left by way of 
explanation at s, and when s is explored in the search, the appropriate follow-up 
expert is invoked. The exact choice for which expert is invoked will depend on 
the history of planning (typically, what neighborhoods and directions have been 
explored from so and s), and on the local geometry of C-surfaces about s. 

Suppose that all experts moved the robot as far as they could, that is, moved until 
a constraint was hit and left the robot touching the constraint. This could result in 
jamming the robot up against many C-surfaces at once. It can prove very difficult 
to extricate the robot from this logjam situation. In fact, it is usually not preferable 
to move all the way up to an obstacle. Instead, we wish to detect this intersection 
with a planned trajectory p, and then back off from the obstacle boundary (along 
p). Thus if p(0) = X and p(l) = Y is the first intersection of p with C-Space 
obstacle boundary, then it makes good sense to move to p(0.8). This has the effect 
of leaving the robot in the channel between obstacles instead of jamming it up in 
corners. Of course, if it is necessary to move to p(.95) then the greedy and Bumble 
strategies will ultimately converge. 
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Figure 2.4. An idealized illustration of the hit and slide strategy. Some expert moves the robot 
in direction t; until a C-surface S is hit at X. When the planner tries to move from X, the 
sliding expert is invoked to slide along S in the goal direction. 

2.4.4. The Intersection Expert 

The mathematics of intersection manifolds in 5R 3 X 50(3) is presented in 
chapters 3 and 4. The intersection expert attempts to find two C-surfaces in A 
whose intersection manifold contains a path which makes progress towards a subgoal. 
The path may be a pure translation or a pure rotation. We will begin by describing 
the process of finding a translational path which slides along an intersection 
manifold. First, all C-surfaces in A which are nearly translationally tangent to the 
goal-direction are selected. We select the first few of these which have the smallest 
value at X. Ideally, these are the closest non-redundant constraints at X. Call 
this set A'. The explanations for the moves from So to s and from s to any sons 
of s will yield a set of previously explored intersection manifolds. (An intersection 
manifold may be identified by the name of the intersected C-surfaces, their levels, 
and the chosen parameterization). The C-surfaces in A' are pairwise intersected 
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(see chapter 4), after appropriate pruning as indicated by previously explored 
intersection manifolds. Each intersection manifold (ker /) f|(ker g) is constructed. A 
translation or rotation vector vj ig is chosen such that the path P/, g {t) = X + tvf t g 
slides along the intersection manifold of the two level C-surfaces ker / and ker g 
at X . The intersection expert then selects the direction t>/ jS which is closest to 
the goal direction (and which is not pruned out by consideration of the planning 
history). Suppose vj i9 is a pure translation. The local operator Translate is called 
to move from X in direction vj j9 until a C- surface is struck 4 or the point on the 
trajectory py )9 which maximizes proximity to the goal is reached. 

Now, suppose Vf >g is a pure rotation. Our experimental implementations have 
intersected two C-surfaces ker/ and ker g to yield pure rotational paths sliding 
along the intersection manifold of ker ff] ker g (see chapter 4 for the details). In 
Proposition (4.4), we demonstrate that these paths may be approximated to an 
arbitrary resolution by successive applications of the local operators, with only a 
linear increase in the number of path segments as the resolution grows finer. We 
have also found it useful to approximate the rotational path along the intersection 
as follows. 

Given two level C-surfaces ker / and ker g at configuration X , we wish to 
choose a direction from X tangent to both. For example, if the configuration space 
were isomorphic to 3? 3 , then ker / and ker g would both be two dimensional surfaces 
in 3-space, and this direction would be V/(X) X Vg(X). (Where X denotes the 
standard cross product on 3? 3 ). In the tangent space to a six-dimensional C-Space, 
there are typically four such tangent vectors at X which are tangent to ker / and 
kerg. We will demonstrate an operator analogous to X which produces one such 
tangent vector in a natural way. (It is also possible to solve for all such tangent 
vectors). 

We begin be defining an extended product on the tangent space to 3i 3 X SO(3) 
at X. Let V = (V x> Vq) G T x be a tangent vector at X. We may think of V x and 
Ve as the translational and rotational components of a six-dimensional velocity 



'Although wc also employ the backing off heuristic here. 
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vector V at X. If W — (W x , W&) 6 T x is another tangent vector at X, we define 
the extended product of V and W by 

V XW = {V X X W x , V© X Wq). 

The cross products on the right hand side are simply the standard three-dimensional 
cross product. (See below (2.3) for why this makes sense for the rotational 
components, V e X Wq). If V = V/ and W = Vg then V X W is tangent to both 
ker/ and ker g at X. Since X only operates on tangent vectors to 8? 3 X 50(3) 
which have the same point of application, we will never have reason to confuse it 
with X , which can only be applied to three-dimensional tangent vectors. 

Let /, g G A' be C-functions generating the C-surfaces ker / and ker g at X. 
Observe that the tangent vector V/(X) X Vg(X) is tangent to both ker/ and 
ker £ at X. We can locally approximate a pure rotational trajectory sliding along 
the intersection of / and g by a path in direction 

*e(V/(*)) X *e(V0pO). (2.3) 

Note that this is well defined since 

(f^l ^L ^l ^L d ± ^l\\ = ( d l 2L ^L\ 

n \\dx' dy' dz' di}>' 80' d<l>)) \d^ 80' 8<t>)' 

The differential rotations from X are isomorphic to a three dimensional vector 
space, and hence the cross product 

is also well defined, and guaranteed to be tangent to ker/ and kerg at X. The 
Rotate operator can be called in succession on the largest components of (2.3) 
in order to approximate the sliding trajectory. Of course, it is also possible to 
re-evaluate the tangents after each step. 
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2.4.5. The Sliding Expert 

The sliding expert attempts to find a path sliding along one level C-surface 
at X, which makes progress towards the goal. The sliding expert can be thought 
of as a less constrained version of the intersection expert. The sliding expert tries 
to choose a C-surface in A' to which the goal-direction is (almost) tangent. As we 
will see in chapter 4, it is possible to choose a parameterization along a C-surface 
which maximizes progress. This path along the C-surface can then be realized (at a 
desired resolution) by successive applications of the local operators. However since 
there are many paths from X sliding along a C-surface at X, we need to develop 
a good heuristic strategy. 

Our motivation is as follows. There are an uncountable number of paths from 
X sliding along a C-surface at X. We could maximize a directional derivative at 
X to choose a locally optimal search direction. This would work once; however, 
this would not solve the problem of state: it is necessary to partition the set of 
paths into "neighborhoods," and to mark a neighborhood of paths as explored 
when a representative from that neighborhood is selected and attempted by a local 
operator. In principle, a computation involving homotopic equivalence classes is 
possible (see Donald (1983a) and appendix III). However, this requires a global 
computation in C-Space. In particular, the image of all paths in an equivalence 
class may cover 3? 3 X SO(3), even if there are several classes. We wish to find a 
way to partition the paths from X into neighborhoods, sample a canonical element 
from the neighborhood, and evaluate it as a local move in the search. 

Given a C-surface normal V/ at X, we wish to choose a direction v sliding along 

A A A 

the C-surfacc ker / which maximizes progress to a subgoal. Let B — (x, y, z, tfi, 0, <p) 
be the obvious orthonormal basis for the tangent space to 32" X 50(3), and 

A A A 

—B = (-x, -y, -z, —if>, -0, -<£). 

Next, we form a set of vectors orthogonal to V/(X) as follows: 



D = {v/po}®(sU-a) 
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where P ® Q ={p X q \ p € P, q £ 2}- All of these vectors are orthogonal to 
ker/ at X. We then choose the direction v 6 D which maximizes 4>x(u,(G — X)), 
where the G — X is the goal direction. If $x is the heuristic product on tangent 
vectors insead of the single- valued Riemannian inner product, then both components 
of the image of 3>x should be maximized. In chapter 4 , we will see that it is 
possible to comply as closely as desired to the C-surface ker/ while traveling in 
direction v. 

To understand this strategy, consider the following example: Suppose we 
employ a basis B' which only spans 3J 3 . Then the expert will choose the available 
translation sliding along the level C-surface which maximizes progress towards the 
goal. Once the direction v is chosen, the Translate operator is invoked to slide 
along the level C-surface until a constraint is reached. 

There is no need for the basis B to be orthogonal; this was merely adopted for 
the sake of intuitive development. The basis provides a sampling of the function 
space of paths compliant to the C-surface about X. 

A Conjecture on Completeness using Extended Spanning Sets 

By using the basis B, we obtain a 12- way sampling of the space of directions 
orthogonal to V/ at X — in other words, there are 12 vectors in D. Imagine using 
another set of vectors, B + , which is larger than B, to construct D. Then D would 
provide a finer sample of the space of directions, since more directions would be 
sampled. In principle it should be possible for a sample to be complete at a given 
resolution. We formalize this idea as follows: 

A spanning set for a space V is a set of vectors which spans V yet which is 
not necessarily a basis. A spanning set is a basis for V which has been extended 
by adding other vectors. We conjecture that there exist certain spanning sets 
which might be employed to construct a complete planning algorithm without the 
Bumble strategy. What constitutes such a complete spanning set? The analogue of 
resolution for an arbitrary spanning set B + would consist in (1) the cardinality of 
the spanning set and (2) the uniformity of distribution of the vectors 
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about the unit five-dimensional sphere S* in tbe tangent space at X. The greater 
the number of vectors in the spanning set, and the more unstorm their distribution 
about S*, the finer the resolution of the planner. The development of such a planning 
algorithm requires surmounting additional theoretical and technical difficulties. 
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Figure 2.5. An idealized illustration of the hit and rotate strategy. Some expert moves the robot 
in direction v until a C-surface 5 is hit at configuration X. When the planner tries to plan^ a 
move from X, the rotation expert is called to calculate a rotation away from 5 (in direction <j>). 
From the new configuration, direction v can be pursued again. 



2.4.6. The Rotation Expert 

The rotation expert is built on the rotational operator Rotate, and is designed 
to handle some of the special problems of moving through rotation space that are 
discussed in chapters 3, 4, and 5. The rotation expert might be called to accomplish 
a simple rotational subgoal, or in conjunction with some more elaborate strategy. 
In particular, when a translational motion terminates by striking a C-surface, 
forwarding messages are left for both the sliding expert and the rotation expert. 
The former has been discussed as the "hit and slide" strategy (figure 2.4); the latter 
is known as the "hit and rotate" technique (figure 2.5). 

The first problem that the rotation expert must deal with is the "wrap around" 
in rotation space. A subgoal <f> can be reached in directions -f <j> and — <f>, although 
typically one is "shorter". In conjunction with the planning history, the rotation 
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expert, on successive applications to the same node, can develop strategies for 
rocking back and forth on a slice of rotation space. 

The Rotate operator is more constrained than the Translate operator (in that 

AAA 

it can only be applied in ±ip, ±9, and ±4>). Hence the rotation expert must have 
a method for approximating rotational trajectories (specified in angle space) which 
are linear combinations of the rotational basis vectors, such as 

v = aV> + bO + cj> (2.4) 

for some scalars a , b, and c. 

In terms of the completeness of the algorithm, there is no need for a rotate 
operator in direction (2.4) (provided a path along v lies in open sets of free space). 
In chapter 4, we show that a continuous path may be approximated as closely as 
desired by a sequence of moves along the rotational axes, and that the number 
of staggered path segments required grows only linearly as the resolution becomes 
finer. In practice this use of the restricted rotate operator has proved adequate 
in our path-finding experiments. However, it is heuristically useful to realize such 
paths as accurately as desired, since this allows higher level experts to suggest 
arbitrary rotational trajectories. Given such a trajectory, the rotational directions 
are ranked by magnitude of change, and the unexplored direction of greatest change 
is first attempted. On failure, or upon successive applications of the rotation expert 
to the search node, the other directions in (2.4) will be attempted. This process 
leads to the approximation of arbitrary pure rotations by a staggered sequence of 
rotations along the axes. If the extent of each rotation is limited, the approximation 
can be made arbitrarily fine. To approximate motion in a direction such as (2.4), the 
planner actually attempts several of the directions simultaneously, which results in 
a spanning "box" of rotational moves about the idealized trajectory (in the absence 
of obstacles). 

Suppose a, b, and c in the idealized trajectory (2.4) are positive. This yields 
a set of positive, or "forward" rotational directions, and a set of "backwards" 
rotational directions which can attain the goal. Which directions are forward and 
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which are backward depend upon the distance (in the vector parameter space V) 
of the goal from X, that is, on ttq(G-X). For example, if G^ — X^ is negative and 
small, then +4> will be a backwards direction, and — will be a forward direction. 

The rotation expert develops and ranks these sets of forward and backward 
rotational directions. By examining the planning history and the local geometry of 
C-surfaces at X, these sets of directions are in turn pruned. In particular, local 
C-surfaces that would block a particular rotational motion are detected. For a 
direction v, this is done by examining the magnitude of the directional derivative 
in v. The importance of such an impediment is then heuristically ranked by the 
closeness of the C-surface at X. Special consideration is given to C-surfaces which 
have a history of proving troublesome. For example, when an expert runs into a 
C-surface, the reason for stopping is left as part of the move explanation. If the 
rotation expert is invoked as part of a "hit and rotate" strategy, then we must 
ensure that the planner tries to rotate away from the C-surface(s) which blocked 
progress. The rotational directions which point away from O-snrfjv.e* may be found 
by examining V/. The process of determining the rotational constraints from the 
local geometry of C-surfaces is closely related to our earlier discussion of detecting 
rotationally orthogonal C-surfaces. 

Thus the requested rotational trajectory and rotational goal provide a set 
of desired rotational motions. The planning history supplies a set of rotational 
constraints, and from the local C-surface geometry can be inferred a set of 
preferred and prohibited motions. The constraints, preferences, and prohibitions 
are intersected with the forward and backward desires. This yields a set of rotational 
directions which will be attempted using the Rotate operator. Depending on the 
kind of invocation, the rotation expert may apply the Rotate operator up to 
some fixed number of times — this is particularly useful when it must attempt to 
approximate an idealized rotational trajectory which is a linear combination of the 
basic rotational directions. 

Canny (1984) has recently extended the Rotate operator for directions such as 
eq. (2.4), corresponding to uniform rotation. 
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Figure 2.6. An idealized illustration of the around expert. When progress for the moving object 
in the goal direction t> is blocked, the expert attempts to find a C-surface which is roughly 
orthogonal to v. A sliding motion (either or -t>) is then planned along this level C-surface 
(around the obstacle). The resulting search node is then expanded. 

2.4.7. The Around Expert 

The around expert attempts to circumnavigate obstacles by sliding around 
their boundary. An idealized illustration of the around expert is shown in figure 2.6. 
The around expert is similar to the sliding expert, except that instead of attempting 
to find a C-surface which contains a path towards the goal, the around expert 
searches for a C-surface which is (roughly) locally orthogonal to the goal direction. 
Next a path is planned sliding along this surface in a direction v orthogonal to the 
goal direction; the path is attempted using a local operator. Typically, this motion 
will result in a search node s' which is farther from the goal than the parent node, 
s. Ordinarily, s' would not be explored soon, since other search nodes would appear 
more promising to the planner's best-first strategy. In order to give the around 
strategy a chance, the around expert explicitly places s' at the front of the search 
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queue and calls the planner recursively. 

The around expert can also invoke the intersection expert. Recall that the 
intersection expert normally tries to construct tangent intersection manifolds which 
contain paths towards the goal. However, when called from the around strategy, 
it can construct intersection manifolds locally orthogonal to the goal direction. To 
construct the intersection set of locally orthogonal level C-manifolds, we perform a 
pairwise intersection of C-manifolds locally orthogonal to the goal direction at X. 

2.4.8. The Suggestor 

The suggestor is a strategy for proposing good subgoals in configuration space. 
As we saw in Donald (1983a), one of the problems with local operators even if 
they are complete (that is, their closure covers configuration space), is that without 
good subgoals, they may take a long time to converge. The suggestor is a heuristic 
strategy for setting subgoals in C-Space. 

First, a very coarse lattice is thrown over C- Space. This lattice is then 
searched for a sequence Q of free configurations (not a path) stepping through the 
lattice to the goal. If no such sequence can be found, then configurations on a 
promising partial sequence are employed. These configurations may then be set as 
subgoals, and the planner can be called recursively. The configurations Q represent 
intermediate planning islands of safe configurations. If paths can be found between 
these configurations, then the find-path problem is solved. Otherwise, expanding 
from any partial paths found can also prove useful, in that the planning islands 
effectively distribute the application of local experts and operators over more of 
configuration space. 

The suggestor complicates the connectivity of the explored neighborhoods 
graph. The ability to explore arbitrary subgoals and suggested paths requires 
more complicated bookkeeping for neighborhood exploration: we must employ the 
connect strategy, in order to know when partial paths link up. If partial paths 
not rooted at the start neighborhood are permitted, then the graph of explored 
neighborhoods will not necessarily be connected, and the mark strategy will fail (the 
mark strategy constructs a directed, spanning tree for a connected, rooted graph 



103 




start 



Figure 2.7. A path which was found using local experts. This find-path problem is very easy (it 
is used as an example in chapter 1). 

of explored neighborhoods). Happily the connect strategy will succeed, since it is 

defined on an arbitrary graph. An algorithm for the connect strategy is discussed 

in section 2.1.2. 
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Listing I: The log of expert explanations for the path in figure 2.7. 



Exploring (-1 
Local Expert: 
Exploring (-6 
Local Expert: 



reaching ((-1 10 1 1 11)) 
ID) 



(find-path *sl *gl) 

Verifying the start and goal points... 
start : (0 1 1 11) , goal : (-6 10 0) . 
Starting search, boss... 
Exploring (0001 1 11) . . . 

Local Expert: I translated straight towards goal, 
1 1 1 11)... 

I Slid along a level C-Manifold, reaching ((-6 10 1 
1 1 1 11)... 

I translated straight towards goal, reaching ((-6 10 1 1 11)) 
Exploring (-6 10 1 1 11) . . . 

Rotation-Expert: Found guiding constraints on rotational motion. 
Rotation-Expert: Intersected Rotational Constraints with desired 
rotations yielding possible motions in 
((MINUS PHI) (MINUS PSI) THETA) . 
Rotation-Expert : I am trying to rotate in (PLUS THETA) . . . 
Local Expert: I rotated to reach ((-6 10 1 1 0)) 



Rotation-Export : 
Local Expert: 
Exploring (-6 10 



Exploring (-6 10 1 1 0) . . . 

Rotation-Expert: Found guiding constraints on rotational motion. 
Rotation-Expert: Intersected Rotational Constraints with desired 
rotations yielding possible motions in 
((MINUS PHI) (MINUS PSI)). 
I am trying to rotate in (MINUS PHI) . . . 

I rotated to reach ((-6 10 1 0)) 
1 0) . . . 

Rotation-Expert: Found guiding constraints on rotational motion. 
Rotation-Expert: Intersected Rotational Constraints with desired 
rotations yielding possible motions in 
((MINUS PSI)) . 
Rotation-Expert: I am trying to rotate in (MINUS PSI) ... 

I rotated to reach ((-6 10 0)) 
0)... 
[success!] Saving and Drawing final path... 
Back to Lisp Top Level in Lisp Listener 2 



Local Expert: 
Exploring (-6 10 
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Figure 2.12. View II: (frame 19), The final configuration. 



2.5. Examples of the Local Experts in Use 

In figure 2.7, we show a very simple example of a path found using local 
experts. Listing I shows a log of the expert explanations for each move. 

The "Thor's Hammer" example in chapter 1 was produced by disabling all 
experts, and employing only the Bumble strategy. (Please refer to this figure). In 
the accompanying figures (2.8-13), we show a path found by a strategy comprising 
all the experts described above. The solution path is very different, and tends to 
slide around obstacles instead of finding convoluted paths between them. 

Figures 2.14-21 show the solution for a find-path problem in a cartesian 
workspace. A cartesian workspace is a bounding box beyond which the reference 
point may not translate. However, the bounding box imposes no restrictions on 
rotations. The Movers' problem in a cartesian workspace is similar to the motion- 
planning problem for cartesian manipulators, and the L-shaped object may be 
thought of as the (wrist and) pay load. First, we show the reference point on 
the L-shaped object. Next two views are presented of the path found within the 
workspace, around a large, diagonally-placed obstacle. View (II) is a view from 
the side; view (I) is a view from the top. Only the back faces of the rectangloid 
workspace are shown. Since the rotation from frames 13 to 14 is very large (> tt 
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Figure 2.8. View I: (frames 1-9). These 18 frames show a solution path for the "Trior's Hammer" 
Mover's problem. Local experts (as described in this chapter) are employed to slide the moving 
object along level C-Manifolds. Three views are shown. The final configuration is only visible iu 
view II (figure 2.12). 
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Figure 2.9. View I: (frames 10-18). 
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Figure 2.10. View II (frames 19) of the Thor's hammer Example using Local Experts. 
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Figure 2.11. View II: (frames 10 18). 
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Figure 2.13. View 111: (A detail of frames 1 through 6). 
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The reference point on the L-shaped moving object 




Figure 2.14. The reference point on the L-shaped object. 



in the — 1/> direction), a detail of the rotation is also shown. 

2.6. Path Planning versus Discrete Intersection Detection 

Imagine a brute-force planner which discretizes configuration space, places the 
robot at every point in the discretization, and tests for intersection. This would 
yield a discrete set of configurations where the robot could be placed. Alternatively, 
the tests could be structured in a search. As stated so far, this is not collision-free 
path planning. Path planning ensures that a path exists between each configuration 
on the path. It has been argued that if the intersection-detection is done at a 
fine enough resolution then a path will have been effectively found. At a given 
resolution, it is possible to bound the size of the intersection between the robot 
and any obstacle which can occur between intersection checks. This bound grows 
smaller as the sampling of the space grows finer. By growing the real-space obstacles 
by this bound, it is possible to ensure that no collisions occur between discrete 
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Figure 2.15. Solution Path, View (II), frames (1-9) 
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Figure 2.16. Solution Path, View (II), frames (10-18) 
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Figure 2.17. Solution Path, View (II), frames (19-21) 
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Figure 2.18. Solution Path, View (I), frames (1-9) 
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Figure 2.19. Solution Path, View (1), frames (10-18) 
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Figure 2.20. Solution Path, View (1), frames (19-21) 
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Figure 2.21. Detail of the rotation from frames 13-14. 



intersection checks (Gouzenes (1983)). Of course, if the resolution is insufficiently 
fine, then the obstacles may be grown so much that no path can be found. 

For gross motion planning in an uncluttered environment, this approximate 
method may perform reasonably well. In complicated environments, however, the 
resolution will have to be fine in order to ensure that paths are collision free 
without growing the obstacles so much that no path can be found. We will 
compare the asymptotic complexity of the discrete intersection method with the 
Rotate operator. (The Rotate operator is the most complex local operator). The 
fundamental observation is that the complexity of the discrete intersection method 
varies linearly with the sampling resolution, whereas the complexity of the Rotate 
operator is independent of (any) resolution. This is because our discretization is 
quite different: a lattice is thrown on the space in order to record the state of the 
planner and the connectivity of the explored neighborhoods. 
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Consider the following. Suppose X and Y are configurations on a rotational 
trajectory in direction <j>. Suppose further that the robot is composed of m convex 
polyhedra containing k generators each, and that there are n convex obstacles 
containing j generators each. The number of faces on a robot polyhedron or an 
obstacle polyhedron is O(k) (respectively, O(j)). To perform one intersection check 
(at a single configuration) for one robot polyhedron and one obstacle polyhedron 
requires time 0(log 2 (; 4- k)) (Dobkin and Kirkpatrick (1980)). This theoretical 
intersection algorithm has not yet been implemented, but we consider it since it 
is the fastest known. To perform a check (at one configuration) for the entire 
robot against the entire obstacle environment requires time 0(mn log [j + k)). Now 
suppose that the path segment [X, Y] must be sampled i times for the quantizing 
intersection checker. This requires time 

0(imn log (j + k)). 

In chapter 5, we show that our planner's Rotate operator could determine whether 
there exists a path from X to Y in time 0(N log N) (where N is the number of 
C-surfaces). In chapter 3, we show that N = 0(mnjk). Hence the complexity for 
Rotate is 

0(mnjk \og(mnjk)) = 0(mnjk(\ogm + logn + log; + log k)). 

Rotate ensures that there exists a path from Xto7 without growing the real-space 
obstacles, and does not involve a resolution factor i. Holding k and j fixed, the 
relative asymptotic performance of the quantizing intersection detector and the 
Rotate operator will depend on whether or not 

i > log(mn). 

The constants i, j, and k will depend on particular workspaces and find-path 
problems. However, we believe that in order to be reasonably sure of the safeness 
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of a path between configurations without growing the real-space obstacles too 
much, i may have to be quite large. This is especially true in reasonable sized 
environments. So as mn increases, the workspace becomes more crowded and/or the 
robot becomes more complicated, and the sampling rate will have to be increased. 
We think it unlikely that the sampling rate will grow only logarithmically with 
the workspace complexity. Moreover, the theoretical 0(log 2 (j + k)) intersection 
time for the Dobkin and Kirkpatrick (1980) algorithm assumes that the solid 
models of the m robot polyhedra are precomputed. (If the solid models must be 
computed for each configuration, then this will take O(km) additional time per 
sample point). In addition, for a polyhedron with k faces, O(fclogfc) preprocessing 
time is required by the algorithm (for each intersection check), which would 
yield an even higher complexity for the discrete path planning algorithm. At this 
stage, since the algorithm is unimplemented, it is unclear whether some sort of 
lazy evaluation, parametric representation, or efficient precomputation could be 
employed to reduce the complexity of iterative application of this intersection test. 
Most implemented intersection detectors that are reasonably robust have time 
complexity 0((j + k)log(j + A;)) or 0((j + k) 2 ). However, it is possible to employ 
minimum distance checks, or 0(j + k) intersection checks in some cases. 

Summary 

In a practical planning system, there are, of course, other considerations. For 
example, our employment of the Rotate operator requires time to update the lattice. 
The main point is as follows: on a lattice of spacing d, to verify the safeness of a 
path of length di, the discrete intersection method requires at least time 0(imn), 
whereas the Rotate operator requires time 0(mn log(ran)) . The discrete method 
actually does not ensure safeness, but merely that the intersection "size" is no 
greater than some function of d. 

Competence versus Performance 

We have shown that the relative performance of the two algorithms will largely 
depend on the constants in the problem. For gross motion in uncluttered workspaces 
the discrete intersection algorithm will probably perform better. In complicated, 
crowded environments, or in problems requiring motions close to the obstacles, the 
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required sampling rate will probably be prohibitive. In addition to the question 
of performance, we should also mention the issue of competence. (In linguistics, 
competence refers to the knowledge base, and performance refers to how well it is 
used). The representations we develop in subsequent chapters are applicable not 
only to the find-path problem with six degrees of freedom, but also to the class of 
geometric planning problems described in chapter 1 (for example, fine motion, and 
planning with uncertainty). It is clear from previous work that these problems are 
within the competence of the representation we develop for 9f 3 X 50(3) (Mason 
(1981), Lozano-Perez, Mason, and Taylor (1983), Erdmann (1984)). At this point we 
have no indication that these problems are within the competence of the discrete 
intersection method. (Find-space, however, can be accomplished using discrete 
intersections). 
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3 

Questions of Representation: C-functions and Applicability 

Constraints in a Six Dimensional Configuration Space 

In this chapter, we first present a formal framework in which several open 
questions about configuration space constraints may be resolved. This framework 
has been discussed informally in the first two chapters. We then proceed to construct 
and prove a set of theorems about the domains and domain topology of C-functions 
for the classical Movers' problem with six degrees of freedom. 

These theorems allow us to define the applicability constraints on C-functions for 
the Movers' problem in 3£ 3 X 50(3). Every C-function characterizes a constraint on 
motion only within a certain region of rotation space. Determining what constraints 
are applicable at a given orientation (or range of orientations) is of fundamental 
importance to the mathematical framework for the spatial planning problem: in 
order to plan using constraints, we must know where (at what orientations) these 
constraints are applicable. Recall that each C-function is generated by a pair of 
boundary cells (a, b), where a lies on the boundary of a moving polyhedron and b on 
the boundary of an obstacle polyhedron. Put simply, the applicability constraints 
determine what boundary cells a and b can interact at a given orientation. 

3.1. Definitions and Conventions 

Let A denote any rigid, convex set. A{&) denotes A rotated to orientation 6. 
Formally, if is an orientation, and £(8) is the corresponding rotation operator, 
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then A(Q) denotes £(©) applied to A. As a kind of shorthand, we refer to A(0) as "A 
at orientation 0," or "A rotated to orientation 0." For example, if F is a face, then 
F(0) denotes F at orientation 0. F's normal, N, rotates with F, and is denoted 
N(Q). We assume face normals are outward-directed from the polyhedra they 
bound. We will in general use A to denote a convex moving polyhedron, and B for a 
convex obstacle polyhedron. If e a is an edge of A and mid(e ) denotes its midpoint, 
then mid(e a (0)) denotes its midpoint at orientation 0. At this point it is not 
convenient to commit ourselves to any particular representation for 3-dimensional 
rotations. However, the reader may without essential loss of generality interpret 
«(0) (for v G 5R 3 ) as the rotation matrix £(0) applied to the vector v, where £(0) 
might be parameterized by Euler Angles. Since £(0) is an orthonormal matrix, 
[^(0)] _1 = [£(0)] T can be employed to rotate a plane which is represented as a 
4-dimensional vector. This operation yields the rotated normal N(Q) of course (see 
Paul (1981)). However, note that the results of this chapter are independent of any 
particular representation of rotations, and that Z(Q) is properly a generic rotation 
operator, u ■ v denotes the standard inner product on 9? >J of u and v. If u and v are 
complicated expressions, however, we will use the notation (u, v). 

The six dimensional configuration space D? 3 X 50(3) is formally defined in 
chapter 2. X will denote a configuration in this space. We will identify with 
£(0) and write £ SO(3). Writing X = (x, 0) makes explicit the translational 
component of the configuration (x) and the rotational component (0 or £(0)). 

d denotes the boundary operator. For example, if F is a face on a polyhedron 
B, then dF denotes the ring of edges which bound F. dB denotes the faces of B, 
de for an edge e denotes e's vertices, and so forth. The coboundary operator is the 
dual of the boundary operator and is denoted S. The coboundary of a vertex is the 
set of edges incident there; the coboundary of an edge are the faces which the edge 
bounds; and the coboundary of a face is the zero, one, or two solids it bounds. In 
chapter 5, we provide a formal definition of boundary and coboundary using the 
chain groups; alternatively, see Hocking and Young (1961) or Giblin (1977). 

We denote the faces, edges, and vertices of a polyhedron B by faces(B), 
edges(B), and vert(B), respectively. 
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If S is a set then i(S) denotes its interior, and kS its closure. kS = i(S)\JdS. 
We denote the classical Movers' problem with six degrees of freedom by SDOF. 

3.2. Representing Constraints in Configuration Space 

Lozano-Perez (1983) showed that the C-Space obstacles can be represented as 
an intersection of a finite number of half-hyperspaces, 1 where each half-hyperspace 
is represented via a constraint function of the form 

fi : D? 3 X 50(3) -> 5K 

where the sign of /;(X) determines whether X is inside, on, or outside the C-Space 
obstacles. However, when rotations are allowed, each constraint function is valid, 
or applicable only within a certain region Ai of the rotation space: 

fi : 5R 3 X Ai -> » {Ai C SO(3)). 

We call such a function / t - a C-function. We consider the robot and obstacles to 
be modeled by the (possibly overlapping) union of convex polyhedra, and define 
a boundary cell to be a face, edge, or vertex of such a polyhedron. C-functions 
model constraints on motion generated by pairs of cells (g (l) gi,) where g a and c/j are 
boundary cells on the robot and on an obstacle, respectively. Lozano-Perez (1983) 
identified three types of interactions: (face/vertex), (vertex,face), and (edge, edge), 
which to preserve tradition we shall term type (a), (b), and (c) constraints. However, 
these interactions can only occur in certain orientations; for example, it is easily seen 
that although two cuboids generate 144 type (c) constraints, at any fixed orientation 
only certain edges can interact and hence only certain type (c) constraints are 
applicable. The region of rotation space where a C-function /,- is applicable is it's 
applicability region, A{. The domain of /;, then, is 5ft X A{. 

For the two-dimensional Movers' problem, the rotation space is the 1-sphere 
and the applicability regions Ai are simply sectors on S 1 . While Lozano-Perez 
'l'W a moving object and obstacles represented as overlapping unions of convex polyhedra. 
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(1983) was able to define the form of C-Space constraints /; for 6DOF, previous 
work has not been able to formulate the applicability regions in 50(3). 

We begin by defining CO C!K 3 X 50(3), the space of forbidden configurations: 

CO = {X |V (X)} (1) 

a 

where C a is a constraint sentence (see Brooks and Lozano-Perez (1983)). a is indexed 
by C-Space obstacles. For each C-Space obstacle O a , C a maps a configuration X 
to true or false, depending on whether X is inside O a . (1) states that if X is inside 
any C-Space obstacle, then it is in CO. 

For X = (x, 0), 

C a (x, 9) = ,\(e € Ai => h{x, 9) < o). (2) 

Let us parse (2). The index i ranges over the set of all C-functions {/i,...,/n} 
which define the C-Space obstacle . We call such a set of C-functions a family 
of C-functions. This family is generated by considering pairwise interactions of 
features on the boundary of A and features on the boundary of B, where A is a 
convex polyhedron on the moving object, and B is a convex obstacle polyhedron. 
For a two dimensional example, refer to figures 1.29-32 (chapter 1), which illustrate 
an obstacle polygon B with four vertices, and a moving polygon A with three 
vertices. For these two polygons, the family of constraints generated corresponds 
to all possible interactions of their edges and vertices: 

family 2 D{A,B) = (faces(A) X vert{B))\j(vert{A) X faces(B)j 

Each pairing, for example (e\,v[), generates exactly one C-function /,-. In three 
dimensions, a family of C-functions corresponds to a set of constraints resulting 
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from the possible interactions of one polyhedral component of the moving object, 
and one obstacle polyhedron: 



family w {A,B) = (faces{A)Xvert{B)\ \j(vert{A)Xfaces{B)\ [j(edges{A)Xedges{B)}. 

Of course, in both two and three dimensions, at a given orientation, only a subset of 
this family is applicable. For each C-function /;, there is an associated applicability 
region A{. Equation (2) for C a can be parsed as follows: for a configuration X, for 
each C-function /,- such thatX is in the domain of /,-, fi{X) must be negative- valued 
(or zero) for X to be inside the C- Space obstacle O a . To determine whether X 
is in the domain of /,-, test whether the rotational component of X is within the 
applicability region A{. 

Next, we define 



F = 5R 3 X 50(3) - CO 

to be the space of free configurations. 

Now, for each C-function / t -, A{ C 50(3) is the corresponding portion of 
rotation space where /,- is applicable. We construct A{ as the intersection of a set 
of half-hyperspaces on 50(3): 

Ai = { 9 € 50(3) | /\( 9j (e) > o) } (3) 

where g 3 - : 50(3) — ► 3J is an applicability constraint function [ACF). A C-function 
fi is said to be applicable for a configuration X = (x, 0) if € A{. In this chapter, 
we will derive, and prove, the form of the ACFs. Geometrically, the applicability 
regions A{ are complicated three dimensional manifolds (with boundary) on the 
projective 3-sphere. Their boundaries are the two dimensional manifolds ker^y. (j 
indexes over the set of functions used to construct A{. There are typically three or 
four gj, as we will see later). 
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The form of the applicability constraints was heretofore unknown. Many of 
the representational and algorithmic issues for geometric planning problems with 
six degrees of freedom rely on a correct formulation of the applicability constraints. 
With these advances, however, the mathematical framework will be complete, and 
we can construct the planner of chapter 2 which exploits the geometry. 

The work of Brooks and Lozano- Perez (1983) dealt with surfaces in the C-Space 
3ft 2 X S 1 , which are called C-surfaces. The obvious extension of this concept for 
dDOF is a C-manifold in 3ft 3 X £0(3). For a C-function /; we define a level 
C-manifold to be the set of configurations X where /; is applicable and fi(X) = I, 
for some level L Thus a level C-manifold is the level set fj 1 ^)- Of particular 
interest is the C-manifold 



kerf i = fi\0) = {X\f i (X) = 0}, 

which contains a boundary patch of a C- Space obstacle. Since in the literature, 
C-manifolds of this form have been called C-surfaces, we shall also employ this 
term. 

We now define paths in C-Space. Given a start configuration s and a desired 
goal configuration g, a successful collision-free path is a continuous function 
p:! 1 ^ 3ft 3 X 50(3) such that p(0) = s, p(l) = g, and p(P) C F. I { denotes the 
closed unit interval, [0, 1]. 

3.3. The Geometric Interpretation for C-functions 

Consider the interaction of an obstacle polyhedron B and a moving polyhedron 
A, where both A and B are convex. Let f p be in the family of C-functions generated 
for A and B. f p models a constraint on the motion of A. For example, f p might be 
generated by considering the interaction of a face of A and a vertex of B. For a 
given orientation ©, the projection into 3ft 3 of any (applicable) C-manifold /~ (0) 
is a plane corresponding to a face of the polyhedron resulting from the Minkowski 
sum of QA and B, that is, 
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B A(Q) = {b + a(0) | b e B, a £ QA } 

where a(0) denotes vector a rotated to orientation © and QA = {—a | a £ A}. 
(Note that in constructing 0A(0), the "negation" takes place before the rotation). 
B © A(0) is the projection into 3? 3 of the C-Space obstacle at orientation 0. In 
effect, we have parameterized the plane equations of faces of 5 0yl(0) by 0. Here 
is the form of the parameterized plane equations derived by Lozano-Perez (1983): 
a;(0) is a vertex of 0A(0) and bj is a vertex of B. Recall that the equation of 
a plane in 9ft 3 can be expressed as { x \ (N, x) = (N, q) }, where N is the plane 
normal and q is a reference point known to be on the plane. Then C-functions take 
the form: 

f p (x, 0) = (N(Q), x) - (N(Q), (a;(0) + 6,-)) (4) 

where a; is a point in 9? 3 . iV(0) is the real-space component of the C-manifold 
normal at orientation 0, and is defined as follows: for a type (a) C-function, 
iV(0) is the normal of a face of QA(Q). For a type (b) C-function, N(Q) is the 
normal of a face on B, and hence is constant. For a type (c) C-function, N(Q) is 
the cross-product of an edge on B and an edge on 0A(0). Furthermore N(Q) is 
normalized to a unit vector when it is non-zero. 

The geometric significance of f p (x, 0), is now clear. The value of f p represents 
how far the (reference) point x lies above the plane of a face in the 0-parameterized 
Minkowski solid. (Assume (x, 0) £ F). When the projection of x falls on the / p -face 
of the Minkowski solid, the metric provided by f p represents the translational 
distance to a collision. When the projection falls outside the face, the value of f p 
represents the translational distance to the plane of the / ?) -face. Hence even though 
there is no convenient way of talking about distances between configurations in 
3? X 5*0(3), we can employ the values of C-functions as a metric on the distances 
of the moving object from obstacles at any configuration. This metric will become 
important in chapter 6. 
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3.4. Redundant Constraints 

In chapter 2, we gave an informal definition of a redundant constraint (see 
figure there). We now give the formal definition of a redundant constraint for 
a configuration X £ F. Intuitively, a redundant constraint is one subsumed by 
nearer, intervening C-functions (lower C-manifolds). Let C denote the set of all 
applicable, positive-valued C-functions at X = (x, ©). For each /,- € C, let s t - be 
the projection into 3? 3 of the kernel of /,• restricted to orientation 0. That is, 

si = {y€® 3 \fi(y,e) = 0}. 

Note that S{ is the projection into 9J 3 of the tangent hyperplane at to the level 
C-manifold for /,-. Intuitively, s t - is the plane of the face of the Minkowski solid 
determined by /,-, at orientation 0. 

Now, let h{ be the half-space of 8J 3 bounded by s t - containing x. Constructing 

t 

yields a solid S in 9J 3 . Those half-spaces bounding S correspond to the non-redundant 
constraints at X. 
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Figure 3.1. 



3.5. Applicability Constraints for type (a) and (b) C-functions 

We are now in a position to derive the domains of the C-functions. To define the 
applicability constraints, we consider a family of C-functions in isolation (that is, 
an environment comprising only the obstacle B and the moving polyhedron A). We 
perform an analysis to see what generators can interact at what orientations. While 
C-functions are defined on the "negated object" QA(Q), applicability constraints 
are defined from the "positive object" A(Q). 

Definition: Consider a constraint c, generated by {g a ,Qb) where the pair {g a ,9b) is 
either (a) a face of A and a vertex of B, (b) a vertex of A and a face of B, or (c) an 
edge of A and an edge of B. We say c is applicable at orientation 9 if some pure 
translation of A(6) can bring g (0) in contact with <fo, such that 

i(ALe))()i{B) = $. 
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See figure 3.1. Let /(O) be a face on a moving polyhedron A(Q), with a normal 
iV(0). Let bj be a vertex on obstacle B. [f,bj) generates a type (a) constraint. Let 
R be the set of adjacent vertices of bj on the edge graph of B, that is, 

R = {b n e vert{B) | 6b n f]5b 3 - ^ }. 

It is instructive to parse the definition for R. (Recall that 6 denotes coboundary). 8b n 
is the set of edges incident at b n . If two vertices b n and bj have disjoint coboundaries, 
then they are not adjacent on the edge graph of B. If their coboundaries overlap, 
then the common element is the edge connecting b n and bj. 

Theorem IJI.l: A type (a) constraint generated by (/, bj) is applicable at orientation 
@ if, and only if, for all b n G R, 

b a ■ N(G) - bj ■ N[Q) > 0. (3.1) 

If the type (a) constraint is applicable, then (3.1) holds for all vertices b n of B. 
We will show that considering the vertices in R provides a necessary and sufficient 
condition for applicability. 

Proof: (•(=) Observe that applicability is invariant under translation. We 
transform the workspace so that the plane of /(0) contains the origin. Then for 
x G 5ft 3 , x • N(Q) is the perpendicular distance of x from the plane of the constraint. 
Since face normals are outward-directed, when this distance is positive, then x lies 
above the plane of /(©). If (3.1) is true, then when bj is brought to rest on the 
plane of /(O), then bj • N(Q) = 0. Now, for all b n G R, b n • N(Q) > 0. Thus all 
adjacent vertices to bj are on or above the halfspace boundary. Since A and B are 
convex, their interiors cannot intersect. 

(=*) If we can bring bj m contact with /(6) while maintaining the disjoint 
interior criterion, then we have bj • N(Q) = 0. No b„ G R can dip below the surface 
of f(0), since then the interiors of A and B would intersect. Hence each b n must 
lie some distance d > above the plane of /(©). I 
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Figure 3.2. 



Nov/, let / be a face of B with normal N. (See figure 3.2). Let a t - be a vertex 

of A, and 

R = { a n £ vert{A) | 6a n f) tfa; ^ } 

be the vertices adjacent to <jj on the edge graph of A. 

Theorem HT.2: A type (b) constraint generated by (a;, /) is applicable at orientation 
if, and only if, for all a n G R, 



a n {&) • N - a t (0) • N > 0. 



(3.2) 



Proof: Symmetric case of Theorem (III.l). 
Consider 



9k(Q) = b n • N(Q) - bj • N(G) 



(3.3) 
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Figure 3.3. The applicability region A e is the intersection of the half-hype rspaces where g k > 0- 
as a mapping g k : 50(3) -»• ft. We call g k a type (a) applicability constraint function 
(ACF). (There are several ACFs for one type (a) C-function— or indeed for any 
C-function, and they are indexed here by k). For the symmetric case from (3.2), we 
call 



g k (G) = a n (Q) • N - o,-(6) • N 



(3.4) 



a type (b) ACF. The region on SO(3) where g k is positive-valued defines a half- 
hyperspace of SO(3) (see figure 3.3). (3.2) and (3.1) define the applicability region 
for a type (a) or (b) constraint as the intersection of these half-hyperspaces. This 
yields the conjunction promised earlier: 



4c = {e€SO(3)iA(»(©)>o)}. 
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A C-function c is applicable if and only if for a configuration (x, 0), each of c's 
ACFs is positive (or zero) at 0, that is, £ A c . The number of ACFs for a type 
(a) or (b) constraint is equal to the cardinality of ' he coboundary of the generating 
vertex (which is the same as \R\). 

3.6. Applicability Constraints for Type (c) C-functions 

Determining the applicability regions for type (c) C-functions (generated by 
edge-edge interactions) turns out to be a bit more grueling. We can derive a set 
of ACFs for type (c) constraints which are analogous to g^ in (3.3) and (3.4). The 
conjunction of these type (c) ACFs is a necessary but not sufficient criterion for 
applicability. The positive conjunction (the intersection of half- spaces where the 
type (c) ACFs are positive) forms two, disconnected regions in 50(3). It will become 
apparent shortly how these regions arise, but let us pause, before bringing in some 
complicated machinery, to survey their topology. In one region A the type (c) 
constraint is applicable, in the other A 1 , it is not. To determine which region is 
in, we use a set of related functions termed disambiguating applicability constraints 
(DACs). Fortunately, the symmetric region A' where the ACFs are positive but 
the constraint is not applicable is disconnected from the valid applicability region 
A (where the ACFs are positive and the constraint is applicable) by a region ~A, 
where the ACFs do not hold (see figure 3.4). We will demonstrate that since A is 
disconnected from A', it is possible to plan continuous paths within A with heed 
only for the basic type (c) ACFs. Both type (c) ACFs and DACs are functions of 
the form g : SO(3) — ► 5ft ; however, they are considerably more complicated than 
(3.3) and (3.4), above. 
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Figure 3.4. The space 50(3) showing A, A', and 1. The type (c) ACFs determine whether « is 
in the set J or in A\JA'. The DACs determine, for G A\JA', wh ether 6 is in A or A '. 

3.6.1. The Basic ACFs for Type (c) Constraints 

Let c be a type (c) constraint generated by the pair of edges {e a ,e b ). As 
usual e a (G) denotes e a rotated to orientation G. We will define type (c) ACFs 
•which provide a necessary criterion for applicability. In conjunction with the DACs 
(below), the type (c) ACFs form a complete characterization of the applicability of 
type (c) C-functions. We employ the following construction: imagine trying to make 
the midpoints of e and e b touch while maintaining the disjoint interior criterion 
for A and B. We then allow A to pivot about 



v = mid(efc) = mid(e a (0)) 



(3.5) 



while maintaining disjoint interiors. Keeping (3.5), for what orientations (values of 
G) are the interiors of A and B disjoint? 
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Figure 3.5. A section view through ej, (ej is orthogonal to the page). 

Let us denote the vertices of e a and ej as follows: (see figures 3.5 and 3.6) 
vert(e a ) = (o,-,a, + i) and uert(ej) = (fey, fey+i). Now, e Q bounds 2 faces /i and fa on 
A; let their normals be Ni and N%. Similarly, let the normals for the faces fa and 
fa cobounding ej be N$ and N4. 

Theorem HI. 3: If a type (c) constraint generated by (e a ,e(,) is applicable at 
orientation G, then 



-rfi(e)d2(e) > 



(3.6) 



and 



-d 3 (e)rf 4 (9) > 



(3.7) 



where 



rfl(0) = fey • Ni{e) - mid(e 6 ) • N^Q) 



(3.6a) 
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Figure 3.6. A section view through e o (0) (e o (0) is orthogonal to the page). 

d 2 (G) = bj ■ N 2 {e) - mid(e 6 ) • N 2 (e) 

d 3 (0) = a,(0) • AT 3 - mid(c a (0)) • iV 3 

d 4 (0) = a t -(0) • N A - mid(e a (G)) • N 4 



(3.66) 
(3.7a) 
(3.76) 



(We express (3.6-7) in this form rather than as di(G)d 2 (G) < in order to preserve 
the positive sign convention for all the ACFs). 

Proof: Refer to figures 3.5 and 3.6. Again, since applicability is invariant under 
translation, we transform the workspace so that mid(et) is at the origin. With 
mid(e a (Q)) fixed at mid(et), d,-(0) for i = 1, 2 is the distance of bj above the plane 
of fi; for i = 3, 4, this is the distance of a,(G) above the plane of /,-. We allow 
e a to rotate about v = mid(et) with 3 degrees of freedom. Observe that a,- and 
a t+ i may not dip below the surface of B, and that bj and 6 J+ i may not fall below 
the surface of A. This is clearly enforced by considering only the planes of the 
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faces cobounding e a and e^. If the type (c) constraint is applicable at 0, then d\(Q) 
and ^2(0) can never both be positive, nor both negative, for in these cases e^ will 
intersect the interior of A. We see this as follows: If c?i(©) < and d-^Q) < 0, then 
bj is inside both halfspaces, and some point on the line segment (mid(efc), bj) must 
be inside A. If d\{&) > and ^(0) > 0, then bj +l is inside both half spaces, and 
some point on the line segment (mid(efr), 6y +l ) must be inside A. 

Hence d\ (0)^2(0) < 0. This immediately yields (3.6). A similar and symmetric 
argument yields (3.7). fl 
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3.7. Disambiguating Applicability Constraints (DACS) for Type (c) 
Constraints 

The basic type (c) ACFs take into account edge-edge interactions, but do not 
model the interactions of the faces they bound. In order to preserve the disjoint 
interior criterion, we introduce Disambiguating Applicability Constraints (DACs) as 
follows. DACs are constraints on the tangent vectors to faces cobounding e^ and e a ; 
assuming that the basic ACFs have determined that Q E A\J A', DACs discriminate 
between A and A'. In fact, the DACs are necessary and sufficient conditions for 
applicability. We split the type (c) applicability computations between the basic 
type (c) ACFs and the DACs for reasons relating to the algebra system, which 
is described in chapter 4. Our proofs draws heavily on constructions employing a 
separating plane. 

The Separating Plane Construction 

Join the midpoints of e a (0) and e^ together as usual. Consider the plane P 
containing v = mid(e a (0)) = mid(ej), whose normal is e a (G) X et- Assume without 
loss of generality that e a (0) X ej 7^ 0. P contains both e& and e a (0). Suppose that 
the type (c) ACFs for constraint c are positive- valued (or zero), i.e., (3.6) and (3.7) 
hold. Hence each vertex of ej, is on or above the plane of one face cobounding 
e a (©), and each vertex of e a (0) is on or above the plane of one face cobounding e^. 
Refer to figures 3.6 and 3.5 once more. By reason of the ACF 1 values for c and the 
convexity of A and B, some open halfspace Pjj of 5K 3 which is bounded by P must 
contain i(B) entirely, and some open halfspace P^ bounded by P must contain 
i(A(Q)) entirely: 

B C k(P b ) 
A(Q) C k(P a ). 

(Recall that k(S) denotes the closure of a set S: k(S) = i(S) [) dS). 

Now, if c is not applicable, then i(A(Q)) (] i(B) y^ 0. This means that A C K>(Pn) 
also, since unless P/\ — Pn, then P would separate i(A(@)) from i(B). We conclude 
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that for all O' E A', i(A(Q')) C Pu and i{B) C Pb- By a symmetric argument, for 
all 0Gi, plane P separates i{A{&)) from i(B). To summarize: If the constraint 
c generated by (e n ,ei) is applicable at orientation 0, then i(A(Q)) f\ i(B) = 0. 
Therefore there exists a separating plane between i(A(0)) and i(-B). On the other 
hand, if c is not applicable, there exists no such separating plane, for then the 
interiors could not intersect. Furthermore, if e a (0) X e^ 7^ 0, then there exists 
exactly one separating plane that contains all four points veri(e a (0)) Uferi(ej). We 
formalize these results in the following lemma: 

In this lemma, we abbreviate e o (0) by e a , and A(Q) by A. 

Lemma HL4.1: (Existence and uniqueness of the separating plane). Join together 
the midpoints of e a and e^. Assume that e a X ej 7^ 0. The constraint c generated 
by (e 0) eft) is applicable if, and only if, the plane P containing e a and e& separates 
the interior of A from the interior of B. 

Proof: (<=) If P separates i(A) from i(B), then i(A)f)i(B) — 0. Therefore the 
constraint c is applicable. | 

Proof: (=>) If c is applicable, then there exists exactly one separating plane between 
i(A) and i(B), and this plane is P. To see this, first observe that if i(A) f| i{B) = 0, 
then by convexity there must exist some separating plane. Assume that this plane 
does not have normal e a X ej,. In this case, the plane cannot contain both e a and 
ej. Since the plane contains the midpoints of both edges, it must intersect either 
e Q or eft in a non-parallel cut. But in this case, the plane intersects the interior of 
either A or B. Thus it cannot be a separating plane. Since there must exist some 
separating plane, it must have normal e a X efc- I 

The strategy for defining DACs is as follows. For each face cobounding e&, we 
choose a point in the interior of that face. The basic type (c) ACFs ensure that 
et is outside the interior of A(Q), and that e a (0) is outside the interior of B; the 
DACs ensure that the faces cobounding e& lie on the opposite side of P from the 
the faces cobounding e a (0). If the type (c) constraint is applicable, then i(A(Q)) 
must lie in a half-space bounded by P complementary to the half-space bounded 
by P containing i(B). The DACs ensure that if the faces cobounding e a (0) lie in 
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Figure 3.7. The Tangent Vectors T 3 and T* to the faces cobonnding cj. 



/c(Pyl), then the faces cobounding ej must lie in k(Pb), with F4 7^ Pg. Since A and 
B are convex, this suffices to show that A and B lie in complementary half-spaces 
bounded by P. 

The vertices of e a (0) and ej lie on P. Let pi, P2 be points in the interior of the 
faces cobounding e a , and ps, p\ be points in the interior of the faces cobounding 
ej,. The DACs ensure that p\ and P2 He on one side of P, and that P3 and p\ lie on 
the other. 

The points inside the faces cobounding ej and e a are chosen as follows. For 
each edge e on B and A, we construct a pair of tangent vectors, [T\,T2), where 
Ti and T^ are tangent and interior to the faces cobounding e. T\ and T2 are also 
perpendicular to e. For an edge e a on A, (7'i(0),r2(0)) will clearly rotate with e a 
and A, maintaining these criteria. The tangent pair for ej is shown in figure 3.7. 
Formally, we proceed as follows: 
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Figure 3.8. The tangent pairs and normals for e o (0). 

Definition: A tangent vector to 5ft 3 (O'Neill (1966)) is a pair (v,p) € 5ft 3 X 5R 3 , 
interpreted as the vector v applied to point p. We will sometimes write v p for (v,p), 
or, when there is no ambiguity about the point of application, we simply write v. 

Definition: Consider an edge e on a polyhedron P. Let /i, h be tnc faces that 
cobound e, and let N\, N 2 be their normals. A tangent pair for e is a pair of tangent 
vectors to 5ft 3 , (T^Zjj), both applied to mid(e). T,- is perpendicular to e and to JV,-, 
and it is directed into the interior of /,• when applied to mid(e) (i = 1, 2). In other 
words, 



Ti = k{Ni X e) 



(t = 1, 2) 



where k G { +1, -1 } is chosen to orient T t - into the interior of /,-. TV,- X e indicates 
the cross product of iV t - and the directed edge vector for c. 
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Refer to figures 3.7 and 3.8. We will now construct DACs. Let (T;j, T4) be the 
tangent pair for e^, and let TV;}, N4 be the normals to the faces cobounding ej. Let 
(Tj(0),T2(0)) be the tangent pair for e„(0), and let iVi(0), iV2(0) be the normals 
to the faces cobounding e„(0). Thus T{ • N{ = (for i = 1, 2, 3, 4). Keeping with 
this numbering convention, let /,- be the face with normal TV,-. As usual, we imagine 
joining together the midpoints of ej and e a (0). 

Let 7Vp(0) be the normal to the plane P, that is, Np(Q) = e a (0) X efc. Assume 
without loss of generality that Np(Q) 7^ 0. The plane containing mid(e u (0)) = 
mid(eft) with normal Np(Q) also contains e a (0) and e^. We construct DACs which 
ensure that i(A(Q)) is on one side of P, and that i{B) is on the other side. To 
ensure that the points mid(efc) + T$ and mid(ej) + J4 lie on the same side of P, we 
have the constraint 

sign(r 3 • 7V>(0)) = sign(r 4 • N P {&)) 

which may be written 

(T 3 • N F (Q))(T 4 ■ N r (G)) > 0. 

Assume without loss of generality that the signs are non-zero. The case where one 
sign is zero is easily handled by examining the other sign. To ensure that the points 
mid(e a (0)) + Tt(0) and mid(e a (0)) + 7'2(0) lie on the same side of P, we have the 
symmetric constraint 

signp^e) • iV>(0)) = sign(T 2 (0) • N P {Q)) 

Now, we must ensure that the two half-spaces are complementary. This is enforced 
by insisting that the signs are opposite. All of the following must be true: 

k„ = sign(T 3 • Np{Q)) 

= signfTd • Np(G)) (3.8a) 

^ = sign(2i(0)-/V/>(0)) 

= sign(r 2 (0) • Np(Q)) (3.86) 

k A =£ k„ (3.8c) 

Equations (3.8a-c) embody the DACs we require. 
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Theorem IQ.4: Let c be a type (c) C-function generated by (e a ,ej). Assume the 
tangent pairs for e*, and e a , and normals to the faces cobounding ej, and e„ are as 
above. Then c is applicable if, and only if, the all the DACs (3.8a-c) hold. 

Proof: (=*) Assume the type (c) constraint is applicable, but that at least one of 
(3.8a-c) is false. We will demonstrate a contradiction. Join the midpoints of e a (0) 
and efc, as usual. If any of the DACs is false, then P does not separate i(A(Q)) from 
i(B): a contradiction. | 

Proof: (£=) We show that if the DACs hold, then c is applicable: if these conditions 
are true, then P is a separating plane. Therefore the interiors cannot intersect, and 
c is applicable. | 
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3.8. On the Structure of the Type (c) Applicability Regions on SO(3) 

In this section, we prove a theorem on the structure of the regions A, A', and 
~A for type (c) constraints, (see figure 3.4) which yields an immediate completeness 
result for our formulation of ACFs and DACs. As promised, we will show that A 
and A' are disconnected on 50(3), and that the region ~A separates them. Our 
proof draws heavily on constructions employing a separating plane (lemma III.4.1). 

Theorem DI.5: ~A disconnects A from A' on 50(3). 

Proof: We first observe that by definition, 

1[J(A\JA') = S0{3) 

(see (3.6), (3.7) for confirmation). Recall the separating plane construction: we saw 
that for all Q' <E A', «'(A(Q')) C Pb and i(B) C Pb- Let P B denote the interior 
cf the complement of Pjy. Py — z(5? 3 Pb)- 3y a symmetric argument, for all 
Q G A, plane P separates i(A(Q)) from i(P>). If A U A' is path-connected, then there 
exists a continuous function, p : J 1 — ► 50(3), such that p(0) = G, p(i) = G', and 
p(/ ! ) C A\J A'. Furthermore, if A (J A' is path-connected, then for all t £ J 1 , either 
i(A(p(t))) C Pb, or i{A{p{t))) C Pb (assume without loss of generality that for all 
*, e 6 X e a (p(t)) ?£ 0). Note that for all t, 

PfVM>(t))) = «. 

Hence in traversing the path p in rotation space, A is required to "flip" over P 
from Pb to Pn, without its interior ever intersecting P. This is clearly impossible 
if continuity is to be preserved. | 

3.9. Orienting Type (c) Constraints 

Consider affixing mid(e a (Q)) to v — mid(e&) as usual. Refer once more to 3.5 
and 3.6. The cross product 

JV>(G) - c a (6) X e b 
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when applied to v will for some point out of Pjj and into Pa; for other 0, 
iV/>(0) will point into Py and out of Pa- (Assume for now that Np(Q) 7^ 0.) Hence 
for some orientations A//>(0) is the correct (unnormalized) real-space normal for 
constraint (e„, e^); for other orientations we must employ —Np(@). When applied 
to v, the real-space normal kNp(Q) (for k £ { +1, —1 }) must always point out of 
Pli and into Pa- The following rule for choosing k is stated without proof: 

k = sign(iVp(0) • 1(B)) (3.12) 

where 7(0) = ^(0) + T 2 (0). 

However, it is easy to see that we need not compute this dot product each 
time we use the C-function. k (and the orientation of Np(©)) will be invariant in 
regions of A where the signs of the ACFs are invariant. For example, if k is positive 
for some (E A and 

d 3 (0) > and d 4 (0) < 0, (3.13) 

then clearly wherever (3.13) holds, then k must be positive. Also, wherever 

d 3 (0) < and </ 4 (0) > 0, 

k must be negative. This argument should be quite obvious if the reader imagines 
how the cross product of the edges changes as e a pivots about mid(ej). This leads to 
the following simple algorithm for orienting a type (c) C-function c. Essentially, we 
can just compute (3.12) once, and record the signs of the ACFs at that orientation. 

(i) For some 0, compute the values of o! t (0) (i = 1, 2, 3, 4) for the type (c) Ad's. 
If c is not applicable, then stop. 

(ii) If k has not been computed yet, calculate k as in (3.12). (Assume k j£ 0). Record 
the signs of ^3(0) and <£t(0) for c. We call this pair of signs the sign map for c. 

(iii) If a k and sign map have been computed for c, then compare the recorded sign 
map to the current sign map for dj(Q) and d^(0). If the sign maps are equal, 
use k to orient c; otherwise use —k. 
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3.10. Singularities and Special Cases 

Our analysis of type (c) ACFs and DACs assumes that e a (0) and ej are never 
aligned, i.e., that their cross-product is never zero. In addition, our algorithm 
for orienting type (c) C-functions assumes that no function d{ is zero. The cross 
product will be zero when e a (0) is parallel to e^, and an ACF will be zero when 
either e a (0) is aligned with a face cobounding ej, or when e\, is aligned with a face 
cobounding e a (0). In practice, these special cases will arise frequently. Fortunately, 
they can be ignored. Consider the following: The vertices of e a generate type (b) 
constraints with the faces cobounding ej; and the vertices of e& generate type (a) 
constraints with the faces cobounding e a . In the cases where e a (0) is aligned with 
€b or a face cobounding ej (or in the symmetric case), some of these constraints 
will also be applicable. In these aligned cases we say that the type (c) constraint 
is subsumed by the neighboring type (a) and (b) constraints, because the disjoint 
interior criterion will be enforced by the type (a) and (b) constraints alone. This can 
be seen as follows: (see figure 3.5) suppose some ACF, for example do, is zero- valued 
at 0, and that £ A. Then both mid(e a (0)) and a;(0) can be brought to rest 
on the plane of fs, while preserving the disjoint interior criterion. Since a J+ i(0) 
is also lies on e o (0), it too may be brought to rest on the plane of fa. Clearly, 
the type (b) constraints generated by (a-i, fa) and (aj-t-1,/3) must also be applicable 
at orientation 0. At this aligned orientation, the type (c) constraint ensures the 
following: while mid(e a (0)) is on the plane of fa, a{(Q)) must also lie on the plane 
°f h • This is precisely the condition enforced by the equivalent pair of type (b) 
constraints. Symmetric arguments hold for the other ACFs. 

3.11. Level ACFs 

For ACFs, there is an analogous concept to a level C-Manifold. Let g : SO(3) — ► 
5ft be an ACF for a C-function c. An ACF Boundary is the space of rotations where 
c is applicable and g is zero: 

kergf = {eeSO(3)| S (e) = 0}. 
A Level ACF is the space of rotations where c is applicable and g is some constant 
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value I: 

{eeso(3)|flf(e) = /}. 

Recall the geometric interpretation for ACFs. Consider a type (b) constraint (see 
figure 3.5). A path p : I 1 — >■ SO(3) along a level ACF for the constraint (0^/3) 
would, if the midpoints of the edges were affixed, preserve Oj(0) at a constant 
height above the plane of fa. 

3.12. A Note on the Computation and Algebra of Applicability Constraints 

The implemented planning system contains an algebra system (described 
in chapter 4), which performs the computations relevant to the applicability 
constraints. We would like to make the computation as simple as possible, for 
otherwise an implementation might be infeasible. 

We have shewn that there are four types of computations fci the applicability 

constraints: 

(i) Type (a) ACFs (3.3) which determine the applicability of type (a) C-functions. 

(ii) Type (b) ACFs (3.4) which determine the applicability of type (b) C-functions. 

(iii) [Basic] Type (c) ACFs (3.6) and (3.7), which provide a necessary but not 
sufficient condition for the applicability of type (c) C-functions. 

(iv) DACs (3.8a-c) which provide necessary and sufficient conditions for type (c) 
applicability. 

However, it is not hard to show that the real-valued functions for (iii) and 
(iv) are composed of simple type (a) and (b) ACFs. We will demonstrate this as 
follows. Let M 3 be the space of normals to planes in 3ft 3 . Note that M 3 is of course 
isomorphic to 9ft 3 . We now define the functions Fa and Fjj to model the computation 
of type (a) and (b) ACFs. These functions will be composed to compute the more 
complex type (c) ACFs and DACs. Let Fa and Fq be real-valued functions 

F A , F b :M 3 X 3ft 3 X M 3 -+ 3? 

where 

F A (b n , bj, N, Q) = b n - N{Q) - bj ■ N{Q) 
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and 

F n {a n , a { , N, 0) = a n (G) ■ N - ^(O) ■ N. 

Clearly, Fa and Fb can be used to compute ACFs for all type (a) and (b) 
constraints. They can also be used to compute type (c) ACFs as follows: 



di(0) 



_ (F A {b 3 ; mid(e 6 ), Ni, 0), if * = 1, 2; 
~~ \F B {ai, mid(e a ), N it 0), if i = 3, 4. 



Np(Q) is already computed as the real-space normal for a type (c) C-function. 
With Np(Q) in hand, DACs can be computed using Fb and Fa- This is because 
DACs are essentially constraints on tangent vectors to the faces of the polyhedra 
in question, and the tangent space of 3ft 3 is isomorphic to its normal space. We 
will show how to compute DACs using type (a) and (b) ACFs. Our trick for 
rotating a tangent vector (u, p) simply involves rotating the line segment (p, v + p) 
to (p(@), [v + p](G)). For example, 

Ti(0) • iV>(@) = Fo(mid(e a (e )) + ri(0o), mid(e o (e )), N P (B), e) 

Here ©o denotes some fixed orientation. Typically ©o is the identity element for 
the rotation group, i.e., it denotes no rotation at all, and will be the orientation 
in which the polyhedra are given, and in which the tangent pairs are initially 
computed. In particular, [7;(@ )](©) = Ti(Q). 

Our reduction of all applicability computation to a few simple functions is 
partially motivated by aesthetics, and partially by the design of an algebra system 
for our planner. The reduction will admit a simpler and more elegant design. 

3.12.1. A Conjecture 

Let us make one final comment on type (c) ACFs. For each type (c) C-function, 
there are two type (c) ACFs. One type (c) ACF (3.6) is the product of two 
type (a) ACFs; and the other (3.7) is the product of two type (b) ACFs. These 
products are constrained to be negative. In practice, we would probably wish only 
to compute the value of each subresult (rf t -) for each type (a) and (b) ACF, and then 
compute a logical conjunction to determine when one is negative and the other 
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positive, instead of computing their product. 2 We conjecture that the composition 
of type (c) C-functions and ACFs reflects the underlying algebraic structure of these 
constraints: observe that each type (c) face /„ b of the Minkowski solid B A{&) 
is the composition (by direct sum) of an edge on A and an edge on B: 

fa,b = e a (e)®e b . (3.14) 

Similarly, the (real-space) normal N atb to such a face is the composition (by vector 
cross-product) of an edge on A and and edge on B: 

N a , b = e a (0) X e b . (3.15) 

In this chapter we have derived a new symmetry, a symmetry for the ACFs of type 
(c) constraints. In particular, it is now clear that type (c) ACFs are the composition 
(by scalar multiplication) of a pair of type (a) or (b) ACFs. 



2 Tbis approach is taken for the implemented planner. 
3 Equations (3.14) and (3.15) are from Lozano-Pcrez (1083). 
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4 
Mathematical Tools for Motion Planning 
in a Six Dimensional Configuration Space 



4.1. Introduction 

Our earlier presentation of representational issues and applicability constraints 

in 9? X S0(3) addressed basic theoretical issues for the motion planning problem. 

In this chapter we discuss specific issues which were critical for the implementation 

of the planning system described in chapter 2. The fundamental issue is the 

intersection problem in high-dimensional configuration spaces: 

(i) How do we intersect high- dimensional level C-Manifolds to construct an 
intersection manifold? 

(ii) How do we intersect a trajectory in configuration space with C-Space constraints? 

Examples and applications of these results may be found in chapters 1 and 2. 

We will proceed as follows. First, as a "simple" example, we will solve these 
problems for the configuration space 9i 2 X S l . For this space the algebra is not 
unreasonable and illustrates some of the complexities of planning for the 6DOF 
case. However, in 3i 3 X 50(3), the equations for some constraints (notably, type (c) 
constraints) can fill several pages. For this reason, I first computed the general form 
of the intersections for an arbitrary constraint, and then solved all intersections 
using Macsyma (LCS (1983)). The results were then optimized and compiled into 
Lisp. For all practical purposes these results are in machine readable form only. For 
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example, using Euler Angles parameterized by = (^,0,<f>) for three-dimensional 
rotations, a type (b) constraint in Macsyma becomes: 

( (-XC (AI) *XC (NG J) *C0S (PHI) -XC (AI) *YC (NG J) *SIN (PHI) ) *C0S (THETA) 
+XC (AI) *ZC (NG J) *SIN (THETA) -YC (AI) *YC (NGJ) *C0S (PHI) 
+YC (AI) *XC (NGJ) *SIN (PHI) ) 
♦COS(PSI) 
+ ( (YC (AI) *XC (NGJ) *C0S (PHI) +YC (AI) *YC (NGJ) *SIN (PHI) ) *C0S (THETA) 

-YC (AI) *ZC (NGJ) *SIN (THETA) -XC (AI) *YC (NGJ) *C0S (PHI) 

+XC (AI) *XC (NGJ) *SIN (PHI) ) 

*SIN (PSI) -ZC (AI) *ZC (NGJ) *C0S (THETA) 
+ (-ZC (AI) *XC (NGJ) *C0S (PHI) -ZC (AI) *YC (NGJ) *SIN (PHI) ) *SIN (THETA) 
-YC (NGJ) *C0S (PHI) +XC (NGJ) *SIN (PHI) +ZC (NGJ) *Z+YC (NGJ) *Y+XC (NGJ) *X 
-ZC (BJ) *ZC (NGJ) -YC (BJ) *YC (NGJ) -XC (BJ) *XC (NGJ) . 

This is the simplest of the constraints; a type (c) constraint is over 10 times as long. 
For 5R 3 X SO(S) our approach has been to (1) derive these constraints (and the 
ACFs) from some arbitrary representation for rotations, (2) reduce each constraint 
to a series of simpler, canonical forms which are linear, bilinear, or quadratic in the 
terms of interest, and (3) develop simple mathematical procedures for operating on 
the canonical forms. 

For example, to construct an intersection manifold for n constraints, we 
essentially need to solve a set of n simultaneous equations, each of the form 

f(X) = 0. (X e 5R 3 X SO{3)) 

We proceed as follows. Let D = { x, y, z, tp, 9, <f> } be the set of all the degrees 
of freedom. First we select P, a subset of 6 — n elements of D. P will parameterize 
the intersection manifold. The variables in P will be the free variables which the 
planner can choose; the variables D — P will vary dependency with P so as to stay 
on the intersection manifold. Mechanically, this entails (l) solving the n constraints 
simultaneously eliminating all but one variable in D — P, and (2) expressing all 
dependent degrees of freedom D — P in terms of the free variables P. 

The canonical forms are expressions for C-functions which make explicit the 
coefficients of the dependent variables (D — P) themselves, and of the sines and 
cosines of these variables. 13 complicated equations describe the canonical forms 
1 Euler angles are implemented as rotation matrices in the planner. See Symon (1971). 



153 



of a C-function, and 9 equations are needed for a type (a) or (b) ACF. 2 Complete 
Macsyma listings of these procedures are provided in an appendix. Before wading 
into these waters, however, let us turn our attention to the configuration space 
D? 2 X S l . 

We will adhere to the definitions and conventions established in chapter 3. 
4.2. The Intersection Problem in 3? 2 X S 1 

The find-space and find-path problems in 9? 2 X S l are of considerable intrinsic 
interest. We have suggested that good algorithms for the two dimensional Movers' 
problem could be developed by planning along the intersections of constraints. Some 
of the necessary theoretical tools for this approach are presented in this section. 
These results illustrate the principles necessary for planning along intersection 
manifolds in 9J X £0(3). The derivations are simpler because (1) the constraints 
are simpler and (2) the applicability regions are merely sectors on the unit circle. A 
complete, general path planner has been implemented for this problem (see Brooks 
and Lozano-Perez (1983)). This section serves both as a pedagogic example and as 
a presentation of a new approach to the planning problem in 0J 2 X S 1 . 

To plan paths along the intersections of constraints, we must be able to construct 
the intersection manifold of some set of constraints. To preserve tradition (see 
Brooks and Lozano-Perez (1983), for example), we will call any level-0 C-manifold a 
C- surface. A C-surface is the space of configurations where a C-function is applicable 
and zero-valued. C-surfaces are interesting because they bound C-Space obstacles. 
We will derive the form of the intersection of any two C-surfaces in 3? 2 X S l . Each 
C-surface is a 2-dimensional manifold in 3? 2 X S 1 , and their intersection manifold is 
a curve p in §ft 2 X S . We derive a curve p which is parametric in 6. Since there are 
2 types of C-surfaces (type (a) and (b)), there are 3 types of intersection manifolds. 

4.2.1. The Intersection of Two C-Surfaces in 3ft 2 X S l 

We describe a technique for finding the intersection of two C-surfaces for the 
two dimensional mover's problem with rotations. Throughout this discussion of 

2 We sec now why it was desirable t.o express all ACFs and DACs as compositions of type (a) 
and (b) ACFs. 

3 ltccall that (x,y,0) is a typical point in the C-Space 9? 2 X S l . 
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3? X S\ we will employ the abbreviations C = cos# and S = sin0. The surfaces 
are embedded in a 4-dimensional manifold and expressed as functions on (x, y, C, S) 
with the added constraint that C 2 + S 2 = 1. A system of equations for two surfaces 
can then be solved for x and y in terms of C and S. 

Two type (a) constraint surfaces are functions of the form f(x,y,0) = 0, for 
example: 



sin(0 + \)y + cos(0 + X;)x — \\bj\\ cos(0 + X t - - 7y) - ||a,-|| cos(X t - - ?/,•) (al) 



sin(0 + \()y + cos(0 + \i)x - ||6j-|| cos(0 + X? - 7y ) - \\a<\\ cos(X/ - r,<) (a2) 

Similarly, two type (b) surfaces are: 

sin(^-)y + cos((£y)x - ||a t -|| cos(0 - <j>j + rji) - \\bj\\ cos(<£y - 7y) (61) 

sin(^.)y + cos(^)* - IK|| cos(0 - # + *,/) - ||6j.|| cos(# - 7 <) (62) 

Refer to figure (4.1). Here the Oj-'s are vertices of the "negated" moving polygon 
(QA in Lozano-Perez [1981, 1983]), in its local coordinate system, rji is the angle 
the line from the origin of that coordinate system to the point a; makes with the 
coordinate system's x axis, and X,- is the angle made by the normal to the segment 
from a,- to a t - + i. Similarly the 6y's are the vertices of a convex obstacle polygon, 7y 
the orientation of the line from the origin to 6y, and (j)j the orientation of the normal 
to the segment from bj to b J+ i. The parameter 6, a parameter of the configuration 
space, measures the angle between the x-axes of the object and obstacle coordinate 
systems. 

Type A constraints can be thought of as being generated by a face (edge) of 
the moving object A coming into contact with a vertex of an obstacle B, and a 
type B constraint as a vertex of A corning into contact with a face (edge) of B. 
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Figure 8. The two types of surfaces can be defined by bring the reference point of the 
negative of moving object A into contact with • vertex and an edge of fixed obstacle B. 
Both are defined over a range of orientations I. 



Figure 4.1. An illustration or the terms in equations (al) and (bl). Reprinted with permission 
from Brooks and Lozano-Perez (1983). 
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Each constraint is valid only over a fixed range of 0. For type A surfaces the range 
is given by 9 £ [(£y_ i — X;, <j)j — \] and for type B surfaces by £ [4>j — X,-, <f>j — X;_ [ ]/ 

By applying trigonometric reductions we can express these constraints as 
follows (only (al) and (bl) are shown): 



cos(\i)Sy + Csin(Xi)y — sin(Xj)5a; + Ccos(X t )x 
+ sin(X t - - 7;/) INI 5 - INI cos (X; ~ Vi) 
-Ccos(X t -- 7 y)||6y|| (al) 



sin(<£y)y + cos(<?!>y)x - ||a,-|| sin(^y - rji)S 

- C\\ai\\ coa(<f>j - rii) - ||6y|| cos(^y - 7 y) (61) 

Where 

C = cos 9, S = sin 9. 

Now, we can consider a pair of these equations as a system in four variables, 
(x, y, C, S), and proceed to solve (al) and (b2), (bl) and (b2), and (al) and (bl) for 
x and y. For example, the intersection of two type (a) surfaces, (al) and (a2) is a 
curve 

P : r ol f>«2 - » 2 X S 1 

where r a if]^o2 C S 1 denotes the intersected applicability constraints for 
(al) and (a2). Although the solutions are in the variables C and S, we can use 
C = cos# = cosr and S = sin# = sinr to generate the curve of intersection 
in 3? X S . Because of their excessive length, these equations may be found in 
appendix I. 

4.2.2. Intersecting Trajectories with C-surfaces 

A General Discussion for VI 2 X S l and 3? 3 X SO{3) 

In order to motivate a discussion of the intersection problem for trajectories 
and C-surfaces, we now introduce the problem in a context which will be expanded 
* Source: The hist three paragraphs are excerpted from Hrooks and Lozano-I'crcz, [1983]. 
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In principle it is possible to intersect arbitrary trajectories with C-surfaces — 
such trajectories could translate and rotate simultaneously. Once an intersection 
is found, we must then determine whether (1) the C-surface is applicable, and (2) 
whether it lies on the boundary of a C-Space obstacle. The question of applicability 
may be resolved a priori by maintaining and updating an accurate set of applicable 
constraints as the planner moves through rotation space. This set is called the 
applicability set. As the planner moves from © to 0', the updating algorithm 
must detect which constraints have expired (ceased to be applicable) and which 
new constraints have been activated (become applicable). The expired constraints 
are deleted from the applicability set, and the new constraints are added. In this 
manner the trajectory will be intersected only with the applicable constraints. 
Another approach involves intersecting the trajectory with all C-surfaces, and then 
finding the first applicable intersection on the boundary of a C-Space obstacle. 
The first strategy is more general in that it decomposes the image of the trajectory 
into equivalence classes where the applicability set is invariant. Hence it can in 
principle be used to map out these equivalence classes on SO(3). However, for 
most environments the latter strategy runs faster, although both techniques can 
be shown to have the same asymptotic complexity. Both algorithms have been 
implemented 5 and tested, and are presented later in chapter 5. 

There are also two ways to determine if an intersection lies on the boundary 

of a C-Space obstacle. Let X be the intersection point of a trajectory with an 

applicable C-surface /. Then X lies on the boundary of a C-Space obstacle bounded 

by / if either of the following holds: 

(i) All applicable C-functions in /'s family are negative or zero- valued at X. 

(ii) If the projection of X into real-space lies within the displaced face of the 
Minkowski solid corresponding to the generators for /. 

Correctness Argument: Let us briefly discuss why (i) and (ii) are equivalent. The 
correctness of (i) is obvious, since the C-Space obstacle is constructed as the finite 
intersection of half hyperspaces, each of which is defined by a real- valued function 
on C-Space. Let S denote the face of the Minkowski solid, and x the projection of 



5 For S 3 X 50(3) but not for ft 2 X S l . 

6 The family of a C-f unction is defined in 3.2. 
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the intersection point into real-space (i.e., X = (x, 0)). We will demonstrate that 
(i) <=> (ii). 

(=*) Suppose (i), but not (ii). We demonstrate a contradiction, x must lie on 
the plane of S, even though x $ S, since that is how the C-functions are defined 
(X could not be an intersection point, otherwise). Recall that the normals of the 
faces (and planes) bounding the Minkowski solid are deflned to be outward-directed 
from the interior. Since the Minkowski solid is convex, the plane of S bounds a 
half-space entirely containing the solid. If x is not within S , then it must be outside 
the plane of some other face, S', which shares an edge with S. But in this case, 
the C-function corresponding to S ' will be positive- valued: a contradiction. 

(■«=) The Minkowski solid is convex. If x £ S , then it is behind (or on) the 
plane of every other faces of the solid. The C-functions are defined in terms of the 
distance of x from these planes, which must be negative (or zero). I 

One further note: suppose that all intersections with C-surfaces — including 
non- applicable C-surfaces — have been sorted along the image of the trajectory in 
C-Space. Then if X is the first intersection for which (ii) holds, then / is applicable 
and X lies on the boundary of the C-Space obstacle. Again, both approaches have 
been implemented, and the results are discussed later. 

Intersecting Trajectories with C-surfaces in 5ft 2 X S l 

We will now present methods for intersecting pure translational and pure 
rotational trajectories with C-surfaces in 3? 2 X S l . Note that as long as every path 
of interest lies entirely within open sets of 9? 2 X S l , then for every such path there 
exists a homotopically equivalent path composed of "staggered" pure translations 
and pure rotations. We assume such paths can be expressed as (piecewise) linear 
functions of some parameter. Intersecting such a path with a C-surface entails 
finding the zeroes of the associated C-function (with respect to the parameter). 

Pure Translational Paths. Note that (al) and (bl) are linear in x and y. At a fixed 
orientation their projection into real space is a line. A pure translational path is 
also a line. Clearly then, intersection of a pure translational path with a C-surface 
is trivial. 
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Pure Rotational Paths. A pure rotational path is a linear function from J 1 to S . 
Intersecting such a path with a C-surface involves finding the zeros (with respect 
to 0) of the C-function at a constant translation. Observe that C-surfaces (al) and 
(bl) are linear in C and S, that is, they can be expressed as 

E l C + E 2 S + E 3 = (4.1) 

where the terms Ei (for i = 1, 2, 3) vary only with x and y. The zeros of (4.1) are 
not hard to find. First we note that (4.1) can be expressed as a pure quadratic in 
C (or S), and that solving a quadratic for its zeros is easy. (We must, of course, 
check for the first applicable zero which is on the boundary of a C-Space obstacle). 
This method is not the best because of susceptibility to numerical problems and 
singularities. Happily, such equations arise frequently in robot kinematics; Paul 
(1981) describes a stable, singularity-free calculation for the zeros of exactly this 
form of trigonometric equation. 

Practical Note 

The reader will notice that motion sliding along an intersection manifold 
in 3i X S will not in general be a pure translation or rotation. We have not 
derived the results for intersecting arbitrary trajectories with C-surfaces in 5ft 2 X S l , 
although in principle it is possible to do so. Note that any such sliding motion 
can be approximated as closely as desired by a sequence of pure translations and 
rotations, and furthermore, any such "approximating" planner will be complete (in 
the sense discussed above) if the "sliding" planner is complete. 

Furthermore, our purpose here is a theoretical analysis in low dimensions 
which still illuminates some of the staggering difficulties in 5ft 3 X 50(3). As it turns 
out, with the additional degrees of freedom in 5ft 3 X SO(3), this turns out to be 
considerably less of a restriction. 

4.3. Related Problems in 5ft 2 X S x 

There are a number of interesting related problems in 5ft 2 X S l . The first 
addresses techniques for "sliding" along one geometric constraint (C-surface). 
Sliding is a useful way to circumnavigate obstacles; it can also be used to slide to an 
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intersection manifold. The second result is of use in the find-space and coordinated 
motion problems, and involves characterizing the minimum clearance to a C-surface 
in. Sf? X S . Again, these results are presented not only for their intrinsic interest, 
but also as an exposition of some of the algebraic techniques required and as an 
illustration of the complications arising in high-dimensional configuration spaces. 

4.3.1. Techniques for Moving Along C-Surfaces in 3ft 2 X S 1 

In this section we present techniques for moving along a C-Surface. We could 
imagine using these methods to move to the nearest "edge" (C-Surface intersection), 
for example. A level C-Surface is defined via a function f(x, y,0) = k for k constant. 
/ is exactly of form (al) or (61) (above), and the level surface in 3ft 2 X S l is all 
points 

L={Xe$t 2 Xr f \f(X) = k}, 

where ry C -S 1 ' is the 6 applicability range for /. 

Define a hyperplane in 3ft 2 X S l as the set 

P = { X e 8ft 2 X S l | X ■ H = -hi }, 

where // = (hi, hi, h$). 

We intersect the level surface Lwith the hyperplane P to obtain an intersection 
curve p : I 1 — ► 3ft 2 X S 1 . The equation for this curve for both type (a) and (b) 
C-surfaces may be found in appendix I. 

4.3.2. Characterizing Clearance to a C-Surface 

It would be very useful to characterize the minimum clearance to a C-surface. 
The result could be applied in the coordinated motion problem to determine where 
two mobile objects could possibly interact. In the find-space problem, we could use 
clearance information to maximize the clearance to a constraint while placing one 
object, in order to leave room for another. We would like to answer the question: 

• For a point b xy £ 8ft", at what orientation is b xy closest to a C-surface, and 
what is minimum directed clearance vector at that orientation? 

Using Lagrange multipliers, we can minimize a function f(x,y,6) subject to a 
constraint g(x, y, 0) — by constructing the auxiliary function 
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H{x, y,0,l) = f{x, y, 0) - lg[x, y,0) 

and simultaneously solving the partial derivatives of H. In our case, g will define 
a C-surface, and / will be a distance function. Now, the rotational dimensions 
cannot be treated uniformly in establishing a metric, so we will define distance in 
Euclidean space. Minimizing the square of the translational distance suffices for 
our purposes. Hence, 

f{x,y,0) = {x-b x f + {y-b y f. 

Differentiating H gives us a system of four equations. Solving these equations for x, 
y, 0, and I is not trivial. We provide the solutions and their derivation in appendix 
I. (Solutions are given for both type (a) and type (b) C-surfaces). 

4.4. The Intersection Problem in 3ft 3 X 50(3) 

In this section we extend the previous examples of intersection problems to 
the 6-dimensional C- space H? 3 X .50(3). At this point we must commit ourselves 
to a particular representation for rotations. The implemented planner uses a 
rotation matrix specified by Euler Angles. Implementing a different representation 
for rotations (such as spherical angles, quaternions, or joint angles for a Cartesian 
Manipulator) would merely require replacing the Macsyma rotation abstraction 
ROTATE-VECTOR with the appropriate new function (and recompiling the algebra 
system). The Euler Angles are 

The intersection problems in 3ft 3 X SO(3) are as follows. With each problem 

we give the motivation for attacking it. 

(i) Intersecting (level) C-surfaces. (Necessary to construct the intersection manifold). 

(ii) Intersecting Level ACFs. (Interesting theoretical question: relates to planning on 
different kinds of intersection manifolds, and exploiting coherence in C-Space 
constraints). 

(iii) Intersecting C-Surfaces with Level ACFs. (Same as (ii)). 

(iv) Intersecting Trajectories with C-surfaces. (Indicates that we may have hit a 
C- Space obstacle). 
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(v) Intersecting Trajectories with ACFs. (Indicates that a constraint has expired 
(ceased to be applicable)). 

Note that we never have to intersect a trajectory with a DAC, since any path 
straying out of a type (c) constraint's applicability region must first violate an ACF 
boundary (see Theorem III. 5). Since all ACFs can be composed out of type (a) and 
(b) ACFs, we need only deal with three distinct kinds of functions on 3?'* X SO(3) 
and two on SO(3) . In the context of this section the term ACF is used to refer 
only to the basic type (a) and (b) ACFs out of which all ACFs and DACs may be 
composed. 

Our approach is as follows: We express all C-functions and ACFs in certain 
canonical forms. The Macsyma procedures to derive these forms are provided in an 
appendix. We then develop certain operations which are defined on any function 
expressed in these forms. Throughout this discussion of 5J 3 X 50(3), we use the 
notation C a = cos a and S a = sin a where qG {ifi,9,<j>}. Most of the claims in 
this section should be self-evident when the rotation matrix R(Q) for Euler Angles 
is considered. 

Claim 4-1-' All C-functions are affine in x, y, and z. This is obvious, since £(0) is 
a linear transformation. 1 

Claim 4-2: While expressions for C-functions and ACFs can contain cross- terms of 
the form C a Sp, S a Sp, or C a Cp, it should be clear that a. =*= f3, that is, C a can 
always be expressed as an affine function of S a . 

To derive this, consider the definition of a C-function (equation (4) in chapter 
3) once more: 

f p (x, 0) = (N(Q), x) - (N(Q), (oi(e) + bj)) 

Only the term (N(Q), a;(0)) could result in any troublesome terms. For a type (b) 
constraint, iV(0) is a fixed vector. For a type (a) constraint, iV(0) is a rotated 
normal of a face of A, and we have 

<JV(0),a;(O)) = (AU t -). 
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Finally, for type (c) constraints, iV(0) is the cross product of e a (0) and e b . This 
results only in cross- terms of different angles: 

K(e),c n (e) X e b ) = <a,(0),(a l+ ,(0) - a t -(9)) X e b ) 

= <a;(0),a; +l (0) Xe b - a t (0) X e b ) 
= (ai(Q),a i+ i(Q)Xe b ) 
= {e b , ai {e) Xa,-+i(e)). 

A proof for the ACFs is very similar. | 

4.4.1. Canonical Forms for C-functions and ACFs 

Definition: The Linear Form for a C-function / : 0? 3 X 5*0(3) — > 3? is an equivalent 
expression 

f(x, y, z, 0) = Ei% + E 2 y + E 3 z + E A , 

where E { : SO{3) -+ 3? (for i = 1, 2, 3, 4). 

Definition: A Trigonometric Quadratic Form (TQF) (in </>) for a C-function / is 
an equivalent expression 

f(x, y, z, i/>, 0, 4>) = F\ sin <j> + F 2 cos <f> + i^, 

where 

F t - : 3? 3 X (V>,0) - 3*. (* = 1,2,3) 

Definition: A Trigonometric Quadratic Form (TQF) (in </>) for an ACF g : 50(3) — ► 
3i is an equivalent expression 

g(\j), 0, <j>) = Gi sin <f> + G 2 cos <£ + G 3 , 

where 

C t - : (iM) -> ». (t = l,2,3) 

The TQFs are defined here in <f> — of course we must also define the TQFs in ip 
and in 6 in the natural way. <f> will be our typical example angle in this discussion, 
however. 
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Before we proceed let us provide some intuition for these definitions. Imagine 
deriving a linear form for a C-function, and setting the expression equal to zero. 
The result is just an expression whose coefficients make explicit how the plane 
equation of the face of the Minkowski solid changes with rotation. 

A TQF (in <j>) is just a way of expressing C-functions and ACFs in terms of 
the coefficients of sin <f> and cos (f>. Linear forms and TQFs will be useful canonical 
forms for the intersection problem in 3J 3 X SO(3). It is important to realize that the 
coefficients E{, F{, and G{ are actually functions on the other degrees of freedom. 

We see immediately from claims (4.1) and (4.2) that: 

Claim 4.3: Every C-function can be expressed as a linear form and as a TQF in 
■0, 0, and <j>; similarly, every ACF can be expressed as a TQF in ip, 0, and (j>. 

4.4.2. Intersecting C-surfaces in *ft 3 X 50(3) 

When intersecting C-surfaces in W 2 X S 1 , we essentially eliminated variables in 
a system of equations. This corresponds exactly to "spending" degrees of freedom 
to comply to two constraints. In #t 2 X S* , there were few choices for which 
variables to eliminate. However, in 9J 3 X S0(3), we have many more degrees of 
freedom, and hence there are more choices for how to solve the intersection of 
a set of constraints. For example, to construct the intersection manifold of three 
constraints, we could spend all the translational degrees of freedom, which would 
result in parameterizing the intersection manifold by (-0,0, </>). Alternatively, we 
could in principle eliminate the rotational degrees of freedom and parameterize 
the intersection manifold by (x, y,z). In the former case, we leave (if;,0,(f>) as 
independent degrees of freedom: parameterizing the intersection manifold simply 
involves solving the 3 constraints simultaneously for x, y, and z in terms of (if), 0, <f)). 
To move along their intersection, we are free to plan any values for (ifi,0,<f>), and 
the parameters for the translational degrees of freedom will vary so as to comply 
to the simultaneous set of constraints. Obviously the choice of which degrees of 
freedom should parameterize an intersection manifold is important; linear forms 
and TQFs give us a general way of attacking it. This approach is best illustrated 
through the following examples: 
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Example (i). A C-surface in linear form is an expression for a C-function in linear 
form set equal to zero. Two C-surfaces expressed in linear form may be intersected 
to yield a 4-dimensional intersection manifold parameterized 7 by (z,ip,Q,(j)). This 
amounts to simultaneously solving the equations 

f(x, y, z,0) = E 1 x + E 2 y + E 3 z + E 4 = 
g{x l y,z,Q) = E[x + E^y + E! i z + E! i =0 

by first eliminating x and then solving for y. This yields expressions for x and 
y in terms of {z,ip,0,<f>); we say that [z , ij) , 6 , cj>) form a 4-parameter family for 
the intersection manifold, and that x and y comply to the C-surfaces / and g as 
[z, i/>, 0, (j>) are varied. 

This intersection has the following geometric interpretation. Imagine holding 
orientation constant at 0i. Then E{ and El are all constant also. Intersecting / 
and g at a constant orientation is equivalent to intersecting two planes in 3? . The 
intersection is a line, and the position along the line may be parameterized by 
the one remaining translational degree of freedom, z. The planes intersected are 
exactly the planes of the faces of the Minkowski solid for / and g at orientation 
0!. 

Example (ii). The intersection manifold f(X) = g[X) = from example (i) may 
be intersected with another C-surface, h[X) = 0, expressed in linear form. Suppose 
z is eliminated. Then the intersection manifold for /, g, and h is parameterized by 
(i/>, 0, <f>). The translational degrees of freedom x, y, and z, will be expressed in terms 
of the rotational degrees of freedom, and will slide along the intersection manifold 
as rotations are chosen. The new intersection manifold f(X) = g(X) = h(X) = 
is a 3-dimensional sub-manifold of 3ft'* X 50(3). This intersection has the following 
geometric significance. Imagine holding orientation fixed at 0] once more. The 
intersection at a fixed orientation of /, g, and h is the intersection of three planes 
in di 3 . This intersection (if it exists) is a typically a point. If 0i is allowed to vary, 
the intersection point moves. The coordinates of the intersection point are the x, 
y, and z degrees of freedom as they comply to the intersection manifold. 
7 Assume that the constraints arc not parallel, and that this is possible, etc. 
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Examples (i) and (ii) show how to spend translational degrees of freedom to 
intersect C-surfaces. In (i), we saw that it is possible to plan motion along the 
the 4-dimensional intersection manifold with one translational and three rotational 
degrees of freedom, (i) can be used to plan a pure translational path complying 
to two C-surfaces. The free translational parameter may essentially be chosen to 
maximize progress in a search algorithm. This is precisely how one "local expert" 
in the implemented planner works. 

One last note on linear forms: the discussion and examples above can be easily 
generalized to arbitrary level C-surfaces (instead of C-surfaces with level 0) by 
increasing or decreasing E\ (the "constant" term in the linear form) by a constant 
equal to the level. 

Intersecting Two TQFs 

Consider a TQF g (in <]>) for either a C-function or an ACF, 8 and suppose 
further that the TQF has been set equal to zero so that it is actually a TQF surface, 
ker g, by which we mean a TQF for a C-surface or ACF boundary: 

F\ sin <$> + F 2 cos <j> + F 3 = 
Such a TQF can be expressed as: 

(Ff + F\) cos 2 4> + 2F 2 F {i cos <f> + FJ - F\ = 0. (4.2) 

The new expression is quadratic in cos<j>. (This explains the name TQF). The 
procedure for intersecting two quadratics is well known. 9 Such a procedure can 
be used to intersect two quadratics of form (4.2) (i.e., with cos<^> treated as the 
quadratic variable). Thus we can obviously intersect any two TQF surfaces. This 
means that the procedure for intersecting two quadratics can be applied to TQFs of 
C-surfaces and of ACF boundaries. This immediately yields an effective procedure 

"Depending on whether the TQF is a C-function or ACF, the functions F t will have different 
domains, but this will not matter for our discussion. 
9 For example, see Winston and Horn (1981), (p. 175). 
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for constructing the intersection manifold of two C-surfaces, two ACF boundaries, 
or a C-surface and an ACF boundary while spending only rotational degrees of 
freedom. 

4.4.3. Intersecting Trajectories with C-Surfaces and ACF Boundaries in 

di 3 X SO{3) 

In this section we extend the method of (4.2.2) for intersecting arbitrary linear 
pure translational and pure rotational trajectories with C-surfaces in 3i 3 X SO(3). 

Pure Translational Paths. A pure translational path can not stray out of an 
applicability region. It is not hard to intersect a linear pure translational path with 
a C-surface. Such a path can be represented as a line in 3? 3 . At the fixed orientation 
of the path, any C-surface can be represented as a plane in 5ft 3 . Hence the problem 
of intersecting a C-surface with a (linear) pure translational path is simply the 
problem of intersecting a line with a plane. The linear form of any C-surface 
directly provides the coefficients of this plane for any (applicable) orientation. 

Note that in intersecting a pure translational trajectory from some configuration 
X 6 F with a set of applicable C-surfaces, we need only consider C-functions which 
are positive- valued at X. 

Pure Rotational Paths. We restrict our attention to linear, pure rotations in 

AAA 

one rotational direction (i.e., in ±ip, ±0, ±<f>), for example, 

4>(t) = k + k x t {t € I 1 ) 

(for some constants k^ and k{). To intersect such a path with a C-surface (or ACF 
boundary), we simply find the zeros of the appropriate TQF. For this example, we 
would use the TQF (in <j>) for the C-surface: 

Fi sin <f> + F 2 cos <j> + F s = 0. (4.3) 

With motion strictly in <£, the functions F{ will be constant, and may be regarded 
simply as the coefficients of a quadratic form. (4.3) is easily solved for the values of 
<j> which are its roots (see section 4.2.2). Now, depending on the solution technique, 
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(4.3) may yield several roots. The correct root may be chosen as follows: for a 
C-surface, we choose the first root where the C-surface is applicable. For an ACF 
boundary, we choose the first root where the associated C-surface is applicable. 
This last step requires examining the other ACFs for the C-surface. 

Completeness and Complexity for Rotational Trajectories 

We have seen that a continuous path through rotation space can be approximated 
as closely as desired by a series of linear motions along the rotational axes. We now 
show that the number of path segments required grows linearly as the resolution 
of the approximation becomes finer. 

Definition: Let V be a vector space, and P and P' be trajectories in V . We say 
that P' approximates P at resolution r if for all p' £ P' , the perpendicular distance 
of p' to P is less than £. 

Proposition 4-4'- A linear trajectory in a vector space can be approximated by a 
number of path segments along the axes, which increases linearly as the resolution 
becomes finer. 

Proof: Suppose V — 3? , and P is a linear trajectory from u to v. Imagine 
approximating P by linear motions along the x, y, and z axes. Segment P into k 
subpaths. From u, attain each of the k — 1 subgoals (and v) by 3 linear motions 
(along x, y, and z) from the previous subgoal. This yields a sequence P' of 3A; 
motions which approximates P at resolution r^. We can bound J- from above as 
follows: 

— < £ max ( K - u x\> \ v y - u y\> \ v z ~ u z \\ 

To achieve a particular resolution r, it is easy to choose the smallest k satisfying 
the reverse inequality. We see immediately that k varies linearly with r. I 

Let the angle space Q 3 be the domain of a chart for 50(3), as described in 
chapter 2. Then the angle space trajectory 

p(t) = Oi +tv 
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for 



v = atf) + bO + C(j) 

specifies a well-defined trajectory %(p(t)) in 50(3). 

Proposition 4-5: We can approximate p as closely as desired by a sequence {<&} of 

1 AAA 

motions in Q along the xj), 0, and (f> directions. Furthermore, the size of the set 
{ Qi } grows only linearly as the resolution r becomes finer. 

Proof: Immediate, from proposition (4.4). | 

4.5. The Algebra System 

The treatment here of the implemented algebra system is mercifully brief. 
Given the discussion, the details, at least in principle, should be easily imagined 
by most readers. In computer algebra these problems are well understood, and the 
system does not make a significant contribution to that field. I would like to note, 
however, that the algebra system is both massive and at the heart of the planning 
system. It takes 12 hours for a dedicated VAX to optimize and compile the vector 
form of the constraints (in Macsyma) into the primitive functions of the Lisp algebra 
system. On top of these primitives is built a more abstract system, which (for 
example) can evaluate constraints, intersect constraints, intersect trajectories with 
constraints, and find zeros of constraints. The intersection and evaluation system 
has automatic singularity handling (for division by zero, imaginary roots, alignment, 
etc). For example, to intersect two C~surfaces (o Id example (i)), the planning system 
will typically specify a list of preferences for the translational parameterization of 
the intersection manifold. The system then attempts to construct an intersection 
manifold with a high-ranked parameterization, and on encountering singularities 
will back up and try again. 

It should now be clear how the algebra system for the planner is designed. 
For each kind of constraint (C-function or ACF), the algebra system contains 
procedures which compute the coefficients of the linear form (for C-surfaces only), 
and coefficients of the TQFs. Each of these procedures can be thought of as a 
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function of (1) the constraint, and (2) the parameters not explicit in the form (for 
example, the rotation parameters for the x coefficient of a linear form). On top 
of this is built a level of abstraction, so that for example the operation "compute 
the sinc^ coefficient of the TQF (in <£)" is defined on all constraints. Coefficients 
of all possible forms are described by a total of 12 coefficients for the linear forms 
of C-functions, 27 coefficients for the TQFs of the C-functions, and 18 coefficients 
for the TQFs of the ACFs. (These functions correspond exactly to the functions 
E{, F{, and G,-, above). All of these functions are constructed and optimized by 
Macsyma running under NIL (Burke (1983)), and then converted into Lisp. 

We have also experimented with precompiling functions for all possible 
intersection manifolds (up to some degree). 10 For intersection manifolds of degree 
2 or 3, this is not hard, and in fact we have already illustrated all the necessary 
mathematics in this chapter. Intersection manifolds of higher degrees may be 
constructed by solving for the submanifold representing the simultaneous satisfaction 
of several constraints, for example, three constraints in linear form together with 
two TQFs such as (4.2). When higher degrees are considered, this becomes 
quite complicated, especially when we allow different parameterizations of the 
intersection manifolds. Construction of intersection manifolds of higher degree may 
be easier when different representations for rotations — such as unit quaternions — are 
employed. This appears a fruitful direction for future research. In practice, we view 
it as preferable, wherever possible, to obtain the values of coefficients of a form 
at a certain configuration, and then to plan locally while keeping these coefficients 
fixed. Thus for example, we might compute the coefficients of the linear forms of 
two C-surfaces at a given orientation, and then intersect the resulting planes to 
obtain a translational path along their intersection. The structure of the forms 

A 

makes this easy to do. For example, rotating the moving object (say, in <p) until 
it hits a constraint is mathematically a complicated operation. All we need do, 
however, is find the coefficients of the TQF in </>, and supply them to a procedure 
in the algebra system which finds the zeros of TQF surfaces. (But see chapter 5 for 
the details of the applicability set computation). 



10 The degree of an intersection manifold is simply the number of constraints intersected there. 
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4.6. Related Issues in 3ft 3 X SO(3) 

4.6.1. Normals to C-surfaces 

Let / be an applicable C-function and X a configuration on a level C-surface 
for /. When an appropriate inner product is defined on the tangent space, the 
normal to the C-surface at X is the gradient of the C-function / evaluated at /. 
Normals to C-surfaces are of great importance for motion planning. The gradient 
may be computed as follows: first the coefficients for the linear form of / (evaluated 
at X) are obtained: 

f{X) = E lX + E 2 y + E 3 z + E A . 

Clearly, §| = E\, |£ = E 2 , and || = E3. To obtain the partial derivatives in the 
rotational direction, we find then coefficients of the TQFs (evaluated at X) 

f{X) = Fi sin <f> + F-2 cos <j> + F 3 

to obtain 

*— = F\ cos <f> — JP2 sin ^. 
a<f> 

4.6.2. C-functions, Potential Fields, Penalty Functions, and Morse Theory: 
A Conjecture 

A popular approximate algorithm for collision avoidance places "potential 

fields" around the obstacles (either in real space or in some C- Space), and attempts 

to navigate the reference point through a trough of least resistance. The obstacles 

may be thought of as having a "charge" which repels the robot, and the goal has an 

inverse charge which "attracts" it. The potential field method is closely related to 

the so-called "Morse Theoretic" 12 approach to motion planning, and lends itself to 

fast control- loop algorithms which can exercise real-time dynamic control of robot 

arms with few degrees of freedom, in simple environments. As might be expected, 

the method works best for robots that can be approximated by points or spheres. A 

proper potential function increases as the robot approaches the obstacle, and goes 

"See sec. 2.4.2 and Krdmann (1984). 

12 Which takes its name from Mornc Theory in differential topology. 
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to infinity at the obstacle boundary. Traditionally, the potential function is chosen 
somewhat arbitrarily, with much emphasis on the closeness of the "fit" of the 
potential surfaces about the real-space obstacles, and with understandable concern 
for the computability of such functions by specific control hardware. With the 
theoretical tools we have developed, it is now possible to give a potential function in 
configuration space which is "exact." For a configuration X, let / be a C-function 
representing the maximum, applicable, non-redundant constraint from one family. 
For each such /, we conjecture that a good potential field function would be: 



loo, if f(X) = 0. 



for some k > 2. Whether or not such penalty functions could be used in devising 
a fast real-time control algorithm is, of course, another question. The suggestion is 
primarily intended to show that there is a representation on which (in principle) 
less approximate potential field methods might be based. 
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5 
Moving Through Rotation Space 



5.1. Introduction 



In this chapter we discuss some of the computational issues involved in 
planning paths involving three dimensional rotations. The primary issue is that 
of keeping track of which constraints (C-functions) are applicable as orientation 
changes. In principle it is possible to intersect paths with all ACF boundaries, and 
thus to determine which applicability regions the path traverses and crosses. It is 
also possible, in principle, to compute the applicability regions a priori, before the 
planning begins. 1 In practice this is computationally infeasible. Even for simple 
environments, there are typically thousands of constraints, each of which has at 
least 3 associated type (a) and (b) ACFs. We will investigate alternative strategies 
which exploit coherence in how the set of applicable functions changes as the 
robot moves continuously through rotation space. In previous chapters (particularly 
chapter 4) Ave showed how to intersect trajectories with C-surfaces and ACFs. 

The applicability set for an orientation © is the set of all applicable constraints 
(C-functions) there. Clearly, there are regions on SO(3) for which the applicability 
set is invariant; orientations in the interior of these regions correspond to orientations 
where no edges or faces of the robot are aligned with the edges or faces of any 



'This approach is similar to the critical region computations suggested by Schwartz and Sharir 
(1981). 
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obstacle. For a fixed orientation 0, we compute the applicability set by examining 
the signs of the ACFs for all C-functions (see chapter 3). However, this is clearly 
not an operation we wish to repeat very often, and the applicability set calculation 
procedure should be memoized. (A memoized procedure records the answer for a 
given input, so it will not have to be recomputed. Instead, it can simply be looked up 
in a table). As the robot moves in rotation space, certain constraints will expire as 
the path moves out of their applicability region, and other constraints will become 
active as we move into their applicability region. This suggests that an incremental 
update algorithm should be possible: we imagine detecting when constraints expire, 
and when new constraints become active and constructing a Deletelist of expired 
constraints and an Addlist of new constraints. The applicability set is then updated 
by means of the Deletelist and Addlist. 

5.2. The Applicability Decomposition for 50(3) 

In this chapter, we will first present a naive algorithm which does not use 
an update strategy. We then present a more sophisticated procedure, called the 
Q update algorithm, which is an incremental update strategy. We have performed 
experiments using both algorithms to implement the local operator Rotate, which 
was discussed in chapter 2. Both algorithms have the same asymptotic complexity. 
Although we have applied both to the find-path problem, they are designed for 
fundamentally different tasks. The naive algorithm is specialized for a particular 
find-path operator, while the update algorithm is a general tool for computing a 
decomposition of C-Space for spatial planning. 

The Q update strategy addresses the fundamental problem of applicability set 
computations in a continuous space. Without the Q algorithm, there exists only 
the "discrete" applicability set computation, which given one point in 50(3) can 
determine the set of all applicable O-functions. With an incremental tipdate strategy 
we can map out regions on 50(3) for which the applicability set is invariant. The 
boundaries of these regions are ACF boundaries. Let ]/(©) be the applicability 
set at © £ 50(3), and ~ be a binary relation on 50(3) such that ~ 8' if, 
and only if ]/(©) = ]/(©')• Clearly, ~ is an equivalence relation on 50(3), and 
50(3) is decomposed by ~ into disjoint equivalence classes where the applicability 
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set is invariant. We call this the applicability decomposition for 50(3). Computing 
this decomposition is a fundamental step in reducing continuous spatial planning 
problems to discrete computational problems. 

We will show how to compute these decompositions for sections of SO(3) in 
any of the directions S — { ±V>, ±0, ±(f> }. In particular: The incremental update 
strategy computes a projection of the applicability decomposition onto a subspace 
of 50(3) which is isomorphic to S 1 . In principle it is not hard to generalize 
these sections to arbitrary rotational slices: algebraically this entails solving the 
intersection of a TQF with an arbitrary pure rotation. As we have noticed, any 
rotation of interest can be approximated as closely as desired be a sequence of 
rotations in S , with no loss of completeness (at a given resolution). 

The naive algorithm, on the other hand, is highly specialized to the particular 
problem of rotating to a constraint. It does not address the more fundamental 
problem of decomposing S0(3) into applicability set equivalence regions. We 
believe that the applicability decomposition is also important to planning problems 
other than find-path, particularly, for find-space, fine-motion, and planning with 
uncertainty. However, in practice the naive algorithm has proved faster for rotating 
to a constraint than any incremental algorithm we have devised. It is gratifying to 
find that both strategies have the same asymptotic complexity; however, we have 
no strong indication that the 0(N log N) bound we demonstrate is optimal, and 
faster algorithms may exist. 

5.3. A Naive Algorithm Without an Update Strategy 

We begin by presenting a naive algorithm for moving in rotation space which 
does not employ an ixpdate strategy. We wish to design an effective procedure which 

A A A A A A 

is to be given a start configuration s, a goal direction a £ { +ip, —ip, +0, —0, +</>, —<f> }, 
and a goal configuration ;/. The goal configuration differs from s only in that the 
goal angle in the a direction will be g n instead of s a . The procedure determines 
if the robot can reach g along the trajectory in 6c, or whether it will strike an 
obstacle, in which case it must return the C-surface hit and the intersection angle. 
(We use intersection angle to mean the value of a at the intersection point). 
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(a) (b) (c) 



Figure 5.1. 



Let C be the set of all C-surfaces. Calculate the intersection of the trajectory 

p(t) = s + ta 

with every C-surface in C (whether applicable or not). Each such intersection can 
be expressed as a single angular value (i.e., the value of t or a for which p(i) 
lies on the C-surface) and hence as a single point on the unit circle. We can 
order the intersection points by their intersection angle with a C-surface. Sort the 
intersections around the circle. Then traverse the intersections on the circle in 
direction a from s, and find the first intersection which is both applicable and on 
the boundary of a C-Space obstacle. In 4.2.2 we gave an algorithm for how this 
may be determined. 

5.4. Update Strategies: Example 

We now proceed to describe how an update strategy works. If constraints could 
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Direction: (MINUS THETfl) 



Start: 2.12; Goal: 8.8 [bold 
] 




Csurface Intersections 





HCF Intersections 



Figure 5.2. As the hammer rotates in the — direction from (V"o,2.42, <£o) to (V'OiOi^o)) the boxes 
in the lower left show the C-Space. obstacle boundaries and ACF boundaries that the trajectory 
hits. Since the hammer is in free-space, it hits no C-surfaces. However, it crosses many ACF 
boundaries. 

expire and become active "arbitrarily", this problem might still be formidable. 
However the following observation makes things much easier: 

Claim (5.1): When a constraint expires, another "neighboring" constraint 
becomes active. 

For example, consider figure 5.1, which depicts a cross-section of a rectangloid A 
moving above an obstacle face /. As A translates, it rotates in direction ^. In 
5.1a, constraint (i>i,/) is applicable, and (v2,f) is not. At 5.1b, however, we move 
out of the applicability region for (t>i,/) and (t>2, /) becomes active. 5.1b is on 
the boundary of the applicability regions, and both constraints are applicable. By 
5.1c, however, (vi,f) has expired. (t>2>/) has replaced (vi,f) in the applicability 
set. (v2, f) clearly seems like a neighboring constraint to (t>i,/), in that v\ and i>2 
are adjacent vertices on the edge graph of A. We would like to devise an update 
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strategy which, given a Deletelist of expiring constraints, could enumerate a small 
list of candidates for the Addlist. In general an expiring constraint will be replaced 
by neighboring constraints. However, the neighborhood function is somewhat more 
complicated than in this simple example. For instance, imagine that A were rotating 
towards the viewpoint (out of the page), leading with vertex u;j (see figure 5.4). It 
is possible for constraints (v2,f), [v\, /), and (t>3,/) to replace (v\,f), if the faces 
/ and {vi,V2,V3,V4 } are parallel when (t>i,/) expires. Clearly v% is also "near" v\, 
but not as near as v% and v\. To exploit claim (5.1), it remains to be seen just 
what we mean by a "neighboring constraint." We should emphasize that the update 
strategy does not predict exactly which constraints will become active, but merely 
a set of candidate constraints, some of which must replace the expiring constraints 
in the applicability set. 

5.5. Using Update Strategies 

Let us modify the naive algorithm to incorporate an update strategy. At 
configuration a, we compute the applicability set. The trajectory p is next intersected 
with all C-surfaces in the applicability set, and with all ACF boundaries for these 
C-surfaces. The two lists of intersections are merged and sorted around the unit 
circle. (The sort key, once more, is the intersection angle). We call this sorted 
structure of C-surface and ACF intersections the intersection queue, since it a 
priority queue containing intersections. An entry in the intersection queue is a pair: 

I C-surface or ACF, Angle of intersection J. 

We then traverse the intersection queue in order from s in direction a, taking 
the following actions when we encounter a C-surface or an ACF intersection: 

(1) When an ACF boundary is hit, a C-surface has expired. Let the angle of 
intersection be a/. Sometimes several C-surfaces expire at once; in this case their 
ACF boundaries will all have the same intersection angle on the circle. Determine 
all the C-surfaces that expire at a] (simply scan down the intersection queue until 
an intersection angle greater than 07 (with respect to direction a) is found). These 
C-surfaces constitute the Deletelist. Assume we have an update procedure, which 
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can determine an Addlist of newly active C-surfaces given a Deletelist of expiring 

C-surfaces. Call the update procedure with the Deletelist, to determine the Addlist. 

(i) Delete all C-surfaces in the Deletelist from the Applicability set. 

(ii) Delete all C-surfaces in the Deletelist from the intersection queue. 

(iii) Delete all the ACFs of C-surfaces in the Deletelist from the intersection queue. 

(iv) Create an Addlist intersection queue, i.e., a sorted structure containing the 
intersections of all C-surfaces in the Addlist, and all ACF boundaries of these 
C-surfaces, with the trajectory p. 

(v) Merge the Addlist intersection queue with the old intersection queue. 

(2) When encountering a C-surfacc intersection, we know the C-surface must 
be applicable, since we have not yet hit an ACF boundary which could invalidate it. 
(This is essentially the correctness criterion maintained by step (l) of the algorithm). 
Test to see if the intersection is on the boundary of the C- Space obstacle. Note 
that this operation typically requires knowing the applicability set. 

We then continue traversing the intersection queue (of course, resuming 
traversal the next a slightly beyond aj in the a direction) until either an obstacle 
is hit or the goal angle is reached. As the intersection queue is traversed, steps 
(1) and (2) are performed to update the queue and detect collisions whenever an 
ACF-boundary or C-surface (respectively) is crossed. 50(3) is typically quite dense 
in ACF boundaries: see figure 5.2. In this figure, the small boxes depict one 
dimensional slices (isomorphic to 5 L ) of rotation space in the — 6 direction. The 
thin line extending out of the circle indicates the start angle, which is — 2.42 
radians, and the heavy line extending out of the circle indicates the goal angle, 
which is — 0. The intersections of the trajectory with C-surfaces are shown in the 
left box (there are none). The intersections of the trajectory with ACF boundaries 
are shown in the right box. Each line indicates the angle of intersection for an 
ACF boundary. The applicability set is invariant between intersection points. The 
moving object is shown rotating between the start and goal angle. The C-surfaces 
and ACFs were generated by the moving object and obstacles shown. However, the 
actual size of each Addlist is usually small. The algorithm works by maintaining a 
correct applicability set as we move in a, and by modifying the intersection queue 
to remove C-surface and ACF intersections that are not applicable. 
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5.6. Update Strategics 

In this section we finally discuss specific update strategies. An update strategy 
has two parts: first, given a Deletelist of expiring constraints, it must predict a 
set of C-functions guaranteed to contain the Addlist. Second, it must test each of 
these predictions to determine which are really applicable. The latter operation is 
conceptually trivial, but since it is expensive, we wish to make the prediction set 
as small as possible. For example, predicting C, the set of all C-functions is clearly 
correct, but not very useful. 

A better approximation would be as follows: given a Deletelist, determine 
all the C-families (i.e., families of C-surfaces) it represents. A safe prediction 
would comprise all the C-functions in these families, since clearly an expiring 
constraint will be replaced by another constraint from its own family. In practice 
this approximation has proved useful, however, it is not the best we can do. In 
particular, note that even two cuboids will generate 48 type (a) C-surfaces, 48 type 
(b) C-surfaces, and 144 type (c) C-surfaces. Clearly the C-family approximation is 
not a very tight upper bound for the replacement set, that is, the Addlist for a 
Deletelist. 

Let ~\)p, Sp, and Tp denote the vertices, edges, and faces of polyhedron P. For 
a moving polyhedron A and an obstacle polyhedron B, we can express the family 
of constraints as: 



Pa X Vn)\J{V A X 7b)Wa X £ n ). 

To be formal, this should, strictly speaking, be considered the domain of a bijection 
C which maps pairs of generators to the function space of C-functions, but 
where there is no ambiguity we will speak of a pair (g^, gp) as representing the 
corresponding C-function C(g J \ ! gp). 

For an expiring C-function (g^, gii), we would like to define a neighborhood 
map on a polyhedron P, 
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(where * is the Kleene star denoting closure) such that the set 

9(9a) X 9(g D ) (5-1) 

is the smallest maximal replacement set for (g^, gr#). In other words, we want (5.1) 
to contain all possible replacement sets for [g\, gu), no matter what the rotational 
motion; but we also wish (5.1) to be as small as possible so as to minimize the 
ACF computations. It is possible for Q to be local in character: although several 
constraints in a family may expire simultaneously, all that we require is that the 
union of their replacement sets is correct. 

We conjecture it might be possible to find exact-or at least smaller replacement 
sets by taking the specific motion into account. Such a strategy has not yet been 
developed, however. 

5.6.1. Mathematical Preliminaries 

In chapter 3, we gave an informal definition (by example) of the boundary and 
coboundary operators. We now define and employ two related operators which can 
be composed to define operators such as "the faces which contain vertices v\, i>2, 
and V3" and "the edges which are incident at the vertices of these faces." 

In this section we define the discrete boundary and coboundary operators. 
Consider a finite collection of cells, S. The discrete boundary and discrete coboundary 
of S, denoted dS and SS, are defined as follows: 



dS = |J ds 

ses 

6S = U 6s. 
ses 

The discrete boundary and coboundary operators have very different properties 
from the normal boundary and coboundary operators. For example, if / is a face, 
then d f — 0, while d f = vert(f). To see this, observe that 



183 



■••2 «*> , ■*■ 

df = d(df) 

= U de 

eedf 
= vert(f). 

In fact, for any "well behaved" object P (and in particular, any polytope), d P = 
and 8 2 P = (this is a fundamental theorem of topology). However, two (or more) 
applications of the discrete boundary or coboundary operator will not, in general, 
yield 0. 

Examples: 8 (i>i, v<2, V3) is the set of faces F which contain at least one of the vertices 
v\, V'2 or V3. Since for one face /, d f — vert(f), then d F = d 8 {vi,V2,vs) 
is the vertices of all the faces F. The set of edges incident at these vertices is 
8d 8 {vi,V2,v$). 

Exercise: What is d 8 (vi,V2,v^)? 

Elementary Review: Boundary, Coboundary, and Star 

We must show that the discrete boundary and coboundary operators are well 

A A 

behaved. We will do so by presenting a formal definition of d (and 8) on a single 
chain. Readers who have encountered a bit of homology will find the demonstration 
transparent. Others may wish to take this section on faith, and to skip to the next 
section, where we define the star operator. 

Discrete boundary and coboundary operators can be considered as the ordinary 
boundary and coboundary "modulo orientation." We see this as follows. (For a 
more comprehensive account see any textbook on elementary topology, for example, 
Hocking and Young (1961)). 

Let K be an arbitrary oriented complex of abstract cells, and Z an arbitrary 
(additively written) abelian group. An n-dimensional chain on the complex K with 
coefficients in Z is a function c n mapping oriented n-cells of K to Z , such that if 
c n (+< 7 ' n ) = ^) then c n {—a n ) = —z. An arbitrary n-chain c n on K can be written as 
the formal linear combination 
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where Z{ = c n (+<r"). The boundary operator d is a mapping from n-chains to 
(n — l)-chains. d(z{ • a") is an (n — l)-chain which has non-zero coefficients only on 
the (n— l)-faces of the cell er™. Formally, let [<T n ,<T n_1 ] be the incidence number for 
a n and a n ~ x , that is 



[a , a J = • 



0, if <r n_1 is not a face of <r n , 

+1, if <r n is a positively- oriented face of a", 

—1, if cr n ~ l is a negatively- oriented face of <x n . 



Hence, 



d( Zi -a?)= V s f^.u" -1 ] • z: • a n ~K 

To factor out the effect of orientation, we define the discrete boundary operator as 
follows: 



$(*■•*?)= ElkV"" 1 ]!-*-* 



n-l 



Discrete coboundary is defined analogously. 
The Star Operator 

Let P be a polyhedron. Any cell k is a face of itself, although it is not a proper 
face. A proper face of P must be lower in dimension than P: If an n-dimensional 
cell k is on the boundary of P, then we call k an proper n-face of P. Thus edges 
are proper 1- faces, and vertices proper 0- faces of a 3-dimensional polyhedron. Let 
K be some complex of cells. If k is a n-face of K , then we write K > k. We will 
usually assume that a face is a proper face. 

Now, let S be some set of cells in K . The star of E (in K) is defined by 
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St(E, K) = { o £ K | (3 t e E), a > r }, 

i.e., the set of all cells in K that contain a member of E in their boundary. When 
there is no ambiguity we will simply write St(E). (Giblin (1977), Hocking and Young 
(1961)). 

For a cell k, define 8 k = k, 8 k = 8k, and 8 k = 8(8k), (etc). We see 
immediately that the star of { k } may be computed as 



n 



St({k})= LUk. 

i=0 

Using this observation, we have implemented the star operator by recording the 
boundary and coboundary of each cell in the geometric model. 

5.6.2. Local Computation of Replacement Sets 

Type (a) and (b) Constraints 

Consider figure 5.3. (i>i, /) denotes a type (b) constraint. Consider any rotational 
motion from the configuration shown. Assume this rotation will cause (vi,f) to 
expire. We wish to determine the maximal possible type (b) replacement set for 
[ v l> f)> that is, the set of neighboring type (b) constraints which could replace {y\, f) 
under any conceivable rotation. 



Consider the set 



(^«!)-{«i})x{/}. (5.2) 



8v\ — 8v\ is just the edges which meet at v\. The discrete boundary of these 
edges is simply the collection of their vertices, vy is deleted, since it is expiring. 
Now, consider a rotational motion which causes (vj,/) to expire. (5.2) will contain 
replacement type (b) constraints. However, (5.2) is not maximal: consider a rotation 
which causes (t>i,/) to expire at some orientation at which a face /' containing v\ 
is parallel to /. (See figure 5.4). Then all the vertices of /' are replacements for vy, 
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/ / / y/" 



Figure 5.3. 



that is, the type (b) replacement set is (vert{f) - { vi }j X {/}• In general, the 
maximal predicted type (b) replacement set for (v lt /) is 



(verf^O-IMJxU}. 



(5.3) 



By similar analysis, we see the following claims: 

Claim (5.8): When («i,/) expires, so will at least one type (c) constraint with 
generators in 

6v x X df. 

Claim (5.4): See figure 5.5. When a type (c) constraint (e a ,e t ) expires, so must 
some type (b) constraint with generators in 

de a X Set. 
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Figure 5.4. (a) A is rotating above face /, out of the page, (towards the eyepoint). /' is the 
visible fare, with vertices v u v 2 , v 3 , and w<. (b), (c) show a section through f as A rotates. 
When / aud /' are parallel, v z , v 3 , and V4 all become active as t>j expires. This is a singular 
point; as A continues to rotate, v 2 and «.< expire, and v 3 remains applicable. The instantaneous 
replacement set for v t is vcrt(f') — { «i }. 

This analysis is, of course, symmetric for type (a) constraints. In this case, / 
would be interpreted as a face of A and vi as an obstacle vertex. The equations 
given all work when the generator pairs are reversed. 

Claims (5.3) and (5.4) are particularly interesting, in that they suggest that 
we can detect all expiring type (c) constraints by examining the ACFs of type (a) 
and (b) constraints alone. 

5.6.3. Definition of the Neighborhood Mapping for the Replacement 
Generators 

The replacement set in (5.3) makes a certain amount of sense: the replacements 
for an expiring generator v\ are to be found in the faces containing v\. On a 
polyhedron P, the general neighborhood function Q is a simple generalization of 
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Figure 5.5. Section through ej. de a = {vi,Vi}. 



(5.3): 



Q{.) = St(vert(-), dP), 



that is, p(k) is the set of all cells which contain vertices of k as faces. 
Let D be a Deletelist. The smallest maximal replacement set for P is 

U Q{9a) X 9{gn)- 

(gA,gn)eP 
This particular formulation requires that we ignore "nonsense" pairings such 
as all members of "V^ X Vjj- This is easily accomplished by appropriate construction 
of the function C mapping pairs of generators to the function space of C-functions. 
We extend the domain of C to dA X dB, and map all generator pairs except those 
in {7 a X V„)U(1>yt X 7bM£a X En) to 0. 
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A Correctness Proof for Q 

We shall now argue that Q is the correct mapping to predict smallest maximal 
replacement sets. Let {gA,Qli) be an expiring constraint at some orientation @. The 
replacement set for a constraint is the Cartesian product of the the replacement 
sets for its generators. Hence the the replacement set for (gA>9lj) 1S r {9A) X r(gfj). 
Consider the replacement set r{g^) f° r 9A> 1 - e -> the set of constraints 

r(9A) X {g B } 

which will replace (<M> £//*)• This set of constraints will become active at orientation 
0, while (gA, gs) expires. Let 

G = {g A }\Jr{g A ). 

Note that (l) all constraints in G X { gn } must be applicable at orientation O, and 
(2) © must lie on the boundary of each of their applicability regions in SO(3). We 
say that at © each g £ G is in ACF boundary condition. Note further that r(gA) 
is not a predictive replacement set, but any actual replacement set for a generator 
g A under some arbitrary rotation. 

We will first show that all g £ G are coplanar. 

All ACFs are defined in terms of a contact vertex and an applicability vertex 
(see chapter 3). The contact vertex is brought to rest on some applicability plane 
(which is parallel to a face of the other polyhedron), and the applicability vertex 
is constrained to lie above that plane. When a constraint is in ACF boundary 
condition, then both the contact vertex and the applicability vertex of at least one 
of its ACFs are constrained to lie on the applicability plane. (We consider type (c) 
constraints to be composed of four such ACFs, two of which are type (a) and two 
or which are type (b) ACFs). In addition, observe that each line segment 



I Applicability vertex, Contact vertex] 
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lies on some edge of either polyhedron A or polyhedron B. In fact, these edges, 
which we term applicability edges, cover the edge graphs of both polyhedra, although 
the mapping is many-one. We are given a generator g& for a constraint {gA,Qli)- 
The constraint is placed in ACF boundary condition. This requires aligning an 
(applicability) edge of A with a face of B (or vice versa). (This point is fundamental 
to understanding the correctness argument: if both contact vertex and applicability 
vertex must lie on the applicability plane, then the applicability edge, which is 
an actual edge of A, must be aligned with the plane). We are then asked to find 
all constraints which can be simultaneously placed in ACF boundary condition. 
This is equivalent to asking, "Given one edge of A aligned with some face of 
B, and maintaining this alignment, what additional edges can simultaneously be 
aligned with faces of B, such that all associated constraints are in ACF boundary 
condition?" (By associated constraints we mean the following: the aligned edge is 
considered as an applicability edge. Since the applicability edges cover the edges 
of the polyhedra, the associated constraints for an applicability edge e^ are those 
C-functions for which the orientation of e^ determines applicability.) 

Now, by fixing an edge e& at some arbitrary aligned orientation (with a 
face of B), we retain one rotational degree of freedom <p about e^. We wish to 
choose this rotation such that (1) the constraint associated with e^ (i-e., (gu> 9li)) 
remains applicable (and of course, in boundary condition), and (2) a maximal set 
of constraints is simultaneously placed in boundary condition. The replacement set 
we compute is the union of these maximal sets. (2) requires a maximal number of 
additional edge alignments, and must also preserve the disjoint interior criterion. 
So choosing <p so as to maximize the number of edge alignments propagates the 
alignment constraint. Clearly, by propagating the alignment constraint, we obtain 
a set of coplanar edges (recall that A and B are convex). Each edge represents a 
contact vertex and an applicability vertex for (one or more) ACFs in applicability 
boundary condition. The associated generators must also be coplanar. 

We have seen that all replacement generators r(g^) must be coplanar with 
gj\. (As usual, there exists a symmetric argument for r(gif)). Given an expiring 
generator gp on a polyhedron P, we wish to predict replacement sets. Replacement 
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sets are obtained from maximal sets of coplanar generators which contain vertices 
of gp. Clearly, the maximal coplanar sets of generators for a convex polyhedron 
are exactly its faces (and their boundaries). Hence, to predict replacement sets, we 
must find the set of faces of P (along with their boundaries) that contain vertices 
of gp. This set is 

9{g P ) = St{vert{g F ), dP). | 

5.7. Analysis and Evaluation 

We have implemented algorithms for moving in some selected rotational 
direction until either the goal or a C-surface is reached: The naive algorithm (see 
section 5.3), the predictive update algorithm based on the C-surface family as a 
loose maximal bound on the replacement set (section 5.6), and the incremental 
update algorithm based on Q (section 5.6.3). We next show that the naive algorithm 
and the Q algorithm both have the same asymptotic complexity. This means that 
their performance will largely HeppnH on the constant factors in the computation. 
We discuss empirical results to indicate the size of these constants. 

5.7.1. Complexity 

Naive Algorithm: 0{n log n) 

Let A r be the number of C-surfaces in the environment (including non- applicable 

C-surfaces). If the moving object is made up of mo convex polyhedra with ko 

generators each, and the obstacle environment comprises no convex polyhedra 

with jo generators each, then clearly N = jokomoriQ. The complexity of the naive 

algorithm is as follows: 

(i) Intersect trajectory with all C-surfaces (O(N)). 

(ii) Sort intersections around S 1 . {0(N log N)). 

(iii) For each intersection, determine if it is applicable and on the boundary of a 
C-Space obstacle. First, test to see if the C-surface is applicable by examining 
its ACFs. If so, there arc two options: (l) if the applicability set is known at the 
intersection point, we can test to see if the other C-functions in the family are 
negative or zero. (2) If the applicability set is not known at the intersection point, 
we can compute the displaced face of the Minkowski solid corresponding to the 
two generators for the C-surface. Next, test to see whether the intersection point 
falls within the face, (l) would make this step O((yo&o)" m rl o) == 0{Ji)k()N). 
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However, (2) needs only examine the generators of a constraint, and allows this 
step to be 0{N). (O(N)). 

We see that the complexity of the naive algorithm is 0(N log N). 
g Update Algorithm: 0(N log N) 

The complexity of the Q update algorithm is as follows: 

(i) Intersect trajectory with all applicable C-surfaces and their ACFs. Let the 
number of applicable C-surfaces be y < N, and the number of ACFs per 
C-surface be a. (y(l + a) intersections = O(^)). 

(ii) Sort the intersections around S . (0(?f- log nr))- 

(iii) For each intersection: Sort, add, and delete j C-surfaces from the intersection 
queue. (0(y log ; )). 

This yields complexity: 



N N % N N 

— log — + Y\ j log y = —(log n — log k) + — (y logy) 



-r + J2 3 lo gy — -rQogN - log k) + — 

K • -j K /C 

= 0{N log N). 
In the next section, we justify treating A; as a constant. 
Actual Performance 

In practice, the naive algorithm has run faster than the Q update algorithm 
for the specific problem of rotating until a C-surface (or the goal) is reached. As the 
complexity analysis has shown, since both algorithms arc 0(N log N), the difference 
in performance will be due to different constant factors. A good estimate for k is 
10. For example, in a typical environment with 624 type (a), 704 type (b), and 1872 
type (c) C-surfaces, 4 sample applicability sets have sizes 353, 362, 365, and 355. j 
is quite small; for this environment it is typically between 2 and 40. The number of 
ACFs per C-surface depends on the degree of the vertices. For trihedral vertices, 
for example, a < 4 (type (c) C-surfaces have 4 ACFs). Hence this tends to balance 
out any possible gains, since k is not much bigger than a. 

Once more we should remember that the Q update algorithm is designed to 
solve the more general problem of applicability decomposition of 50(3), while the 
naive algorithm has been specialized to solve the "rotate to a C-surface" problem. 
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We speculate that similar specialized algorithms may be developed as fast solutions 
to specific spatial planning problems. However, decomposition tools are a more 
general solution which can be applied to a whole class of spatial planning problems. 

5.7.2. Related Work, Searching and Lazy Evaluation 

The implemented planning system is described in chapter 2. The control 
structure of the algorithm is a search. The search employs certain local operators 
for moving between configurations. One such local operator is precisely the "rotate 
to a C- surface (or the goal)" algorithm. 

As for most heuristic 2 search algorithms, an adversary can probably devise a 
find-path problem which must require an exponential amount of time to solve. This 
does not imply that a polynomial-time algorithm using the mathematics presented 
in this thesis could not be devised; indeed, the theoretical work of Schwartz 
and Sharir (1982a) suggests this possibility. However, in practice, the planner has 
performed quite well. We offer the following explanation for why the planner should, 
in average cases, perform better than in the adversary situation. 

In the theoretical work of Schwartz and Sharir (1982a) and Schwartz and 
Sharir (1981), the concept of non-critical regions is introduced. A non-critical 
region, intuitively speaking, is a region in (free) configuration space where the 
constraints are invariant. We employ similar constructs via sets of non-redundant 
constraints, and by means of applicability sets. In Schwartz and Sharir (1981), for 
example, free space is decomposed into critical and non-critical regions, and the 
connectivity of these regions computed. The connectivity graph is then searched 
for a path. However, computing these regions is (geometrically and algebraically) 
quite difficult. The regions are at least as complex as the C-Space obstacle and 
applicability regions. 

Instead of precomputing the applicability regions (or knowing them a priori), 
our planner computes them as it explores configuration space. While in the worst 
case the entire applicability decomposition must be calculated, this case does not 



2 Wc use the term heuristic in reference to the time complexity, and not the completeness of the 
algorithm. 
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6 
The C-Voronoi Diagram and its Relationship 

to Intersection Manifolds 



6.1. Introduction 

For a finite set of points P in the plane, the Voronoi diagram is the set of all 
points in the plane which are equidistant from two or more points in P. The Voronoi 
diagram for P is a network of straight line segments. Drysdale (1983) introduced 
the generalized Voronoi Diagram (or GVD) for the plane: for a set of polygons in the 
plane, the GVD is defined to be all points in the plane which lie (perpendicularly) 
equidistant between two or more polygons. The GVD is a network of straight line 
segments and parabolic sections. If the polygons are considered as obstacles, the 
GVD represents the network of paths through free-space which maximize clearance 
from the obstacles. Brooks (1983a) and O'Dunlaing and Yap (1982), O'Diinlaing, 
Sharir and Yap (1982) have developed definitions and algorithms employing an 
extension of the Voronoi diagram for low-dimensional configuration spaces. Nguyen 
(1983) also discusses the relationship of global methods to the GVD. 

More formally, the generalized Voronoi diagram (and its extensions) decompose 
the free space into a set of regions, {R{}, such that all points X € Ri are closer to 
one obstacle than to any other. Thus points on the GVD are equidistant from two 
or more obstacles. 
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Figure 6.1. A picture of the generalized Voronoi Diagram for a bounded 2D workspace containing 
four polygonal obstacles. Reprinted with permission from Nguyen (1983). 

In this chapter, we extend the concept of the generalized Voronoi diagram to the 
six dimensional C-Space 3? 3 X SO(3), to provide a formal, constructive definition 
of the C- Voronoi Diagram, or CVD. The CVD is an attractive construction, in that 
it contains a representative component for each "branch" of free space. Each such 
component is submanifold of dimension < d < 5, called a Voronoi manifold. We 
will derive the following connection between intersection manifolds and the CVD: 

Let p be a path along the CVD. p lies along a connected chain of Voronoi 
manifolds, Vi, . . . , "V/fc. We demonstrate that for each Voronoi manifold "V,', there 
exists an equivalent intersection manifold of level C-surfaces, J,-. Furthermore, 
we also show that for every connected chain of Voronoi manifolds, there is an 
equivalent connected chain of intersection manifolds (of level C-surfaces). (The 
equivalence we demonstrate is actually stronger that homotopic equivalence). 

This yields an immediate (theoretical) completeness result for planning along 
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intersection manifolds. While our proof is constructive, it cannot be considered an 
effective procedure. The charts for the Voronoi manifolds are undoubtedly very 
difficult to derive. This in turn makes it hard to develop planning algorithms along 
the C- Voronoi diagram. In C-Space, the most attractive feature of the CVD is not 
that it maximizes clearance from obstacles, but that it represents the connectivity 
of free space. In other words, given the CVD, the Movers' problem can be solved 
by connecting the start and goal configurations to the same connected component 
of the CVD. But since the Movers' problem has already been reduced to the task of 
navigating a point, it is clear that, modulo some uncertainty bound, we do not need 
to maximize clearances while in planning paths in C-Space. We demonstrate that 
instead, it is possible, in principle, to devise a planning algorithm along intersection 
manifolds — for which we have derived charts (chapter 4) — which is equivalent to a 
planner along the CVD. 

Generalized Voronoi Manifolds 

In this section we define the C- Voronoi Diagram (CVD) for the configuration 
space 3ff 3 X 50(3). Note that for Euclidean configuration spaces we would employ 
the standard techniques (Drysdale (1983)). The metric in 5? 3 X 50(3) is non-obvious, 
and the CVD does not reduce to the CVD when rotations are factored out. However, 
it has the same connectivity as the GVD. The CVD for configuration spaces without 
a Euclidean distance metric is fundamentally different, and is defined as follows. 

To define the CVD, we rely on the collection of pseudo-metrics provided by 
the geometric interpretation of C-function values (chapter 3). Intuitively, within 
some well-defined region in free-space where a C-function is non- redundant, its 
value characterizes the translational distance to either (l) an obstacle face, or (2) 
the plane of the obstacle face. Formally: 

In this chapter, we will use 7 C 5ft 3 X S0(3) to denote free space. See chapter 
2 for a formal review of charts and atlases. As noted in more detail in chapter 2, in 
this thesis we usually specify charts via the inverse form h : E n -+ M (where E n is 
an open subset of 5ft re ) with the understanding that it is the inverse (or set of local 
inverses) /i -1 which provides the family of charts {(h~\W{)}, for Ui" W»' = h(E n ). 
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Definition (1): Let M be the set of families of C-functions on 9? v * X SO(3). For 
X £ 7, let Ax be the set of maximum, applicable, non-redundant C-functions 
within families, that is, if M £ M is a family of C-functions, and M' C M is 
the subset of applicable and non- redundant C-functions at X, then M contributes 
to A x the function / £ M' such that f(X) > h(X) for all h £ M' - {/ }. If k 
functions in M' tie for maximum, then M contributes all k to Ax- 

Let n be the dimension of C- Space. Now, X £ CVD if there exists a maximal 
subset B of Ax, containing at least two and no more than n C-functions, such that 
all functions in B have the same value feo a t X and all functions in Ax — B have a 
value greater than &o- We say that X £ CVD lies on an (n — \B\ + l)-dimensional 
Voronoi Manifold. The C-Voronoi Diagram for C- Space is the union of these 
Voronoi Manifolds. 

We have seen that a level C-manifold is analogous to a level surface in 3? 3 , in 
that it is the set of configurations {X \ f(X) = £} for some applicable C-function 
/. Clearly, points on a fc-dimensional Voronoi manifold "V lie on the intersection of 
n — k + 1 equal level C-manifolds, i.e., 

h{X) = ... =f n _ k+1 (X) = l(X) 

where the level 1{X) is allowed to vary as X moves along V. Furthermore, we insist 
that the C-functions /; constructing the Voronoi manifold must belong to pairwise 
distinct families. 

When we say that a Voronoi manifold "V; is constructed from a set of constraints 
F,-, we mean that all the C-functions / £ I'\ have equal value along "V{. An intersection 
manifold constructed from F{ is the intersection of level C-surfaces for constraints 
in F{. By this we mean that first a level is chosen for each / £ F{, and then 
the resulting level C-surfaces are intersected. In general, a level C-surface for a 
C-function / at level £ has the form 



r\i). 
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Figure 6.2. Strongly equivalent intersection manifolds, and the bridge manifolds connecting them. 
Each V, is strongly equivalent to I,. Each bridge manifold Z?i,,+i is equivalent (but not strongly 
equivalent) to V<. 

f~ l {t) is the trivial intersection manifold, that is, the manifold constructed by- 
intersecting one level C-surface with itself. The intersection of two level C-surfaces 
is 



Ji = r 1 (Mf1»" 1 («2)- 

The intersection manifold J{ is constructed from the same C-functions at different 
levels: 



n = r\t[)C\g- l {&). 



An Overview of the Proofs 
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Let p be a path along the CVD. p lies on a connected chain of Voronoi 
manifolds. Call this chain "Vi,...,"Vfc. For a Voronoi manifold "V,- we say that an 
intersection manifold 7 t - is equivalent to "V; if (1) the set of C-functions which 
construct "V," is a (possibly non-strict) superset of the C-functions F[ which define 
Ii, (2) 7,- is homotopically equivalent to %, (3) 7,- lies in free space, and (4) all 
C-functions in F[ satisfy definition (l) along 7,-. If F{ = F' it i.e., 7; is constructed 
with exactly the same C-functions, then 7 t - is strongly equivalent to "V,-. 

Note that by definition, each V{ is restricted to where the conditions of definition 
(1) hold, i.e., to some region NR(Fi) C 7 where all C-functions in Fi constructing 
V{ arc maximum, applicable, non-redundant C-functions within families. Now, in 
general, 7,- is an unbounded level set which cannot lie in free space everywhere. 
Thus by convention, we also restrict 7 t - to the region of interest NR(Fl) where 
all C-functions in F\ satisfy definition (l). That is, NR{F' i ) = {J|F|CA X }. 
Instead of writing 7; fl NR(Fj) everywhere, this convention is assumed throughout. 

In is an interesting question whether, for every Voronoi chain "Vi, . . . , ~Vk, there 
exists a connected, finite, corresponding equivalent or strongly equivalent chain of 
intersection manifolds 7i, . . . , Ik 1 (where k is not necessarily equal to k'). Theorem 

(I) shows that for all Voronoi manifolds "V;, there exists a strongly equivalent 
intersection manifold 7;. These 7; might not form a connected chain. Theorems 

(II) and (III) show that each disconnected pair of intersection manifolds 7; and 
7,+i can be connected by an infinite sequence of "bridges." Each bridge is an 
intersection manifold equivalent (but not strongly equivalent) to X'. We then argue 
that since there exists an infinite bridge sequence, therefore there must also exist 
a finite bridge sequence. Finally, (theorem IV) we show that there exists an entire 
intersection chain 

JiU-UJk' 

which is homotopically equivalent to the entire Voronoi chain 

v,u-UT*- 
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Theorem I: Let "Vjt be an ra-dimensional Voronol manifold, constructed from a 
set of applicable, non-redundant constraints A, which satisfy definition (1) along 
"Vjfc. Then if there exists an intersection manifold Ik of level C-surfaces for the 
constraints A, and if the constraints A satisfy definition (1) along 1^, then "Vfc and 
Ik are homotopically equivalent. 

Proof: We will show that two intersection manifolds constructed from the same 
C-functions at different levels are homotopically equivalent. Next, we demonstrate 
that the Voronoi manifold is essentially a special case of intersection manifold. 

Let T4 be an m-dimensional Voronoi manifold, 

Vjb = {*|/i(X) = --. =fn{X)}, (n = 6-m + l) 

where the f{ are chosen from A as defined above (see definition (l)). Note that 
the value of the fi may vary with X £ "V^. Let Jfc be a (m - l)-dimensional (or 
O-dimensional, if m = 0) intersection manifold of level C-surfaces constructed from 
the same functions / t -: 

h = {x\ h{x) = i x , h{x) = 4, • • • , MX) = i n }. 

The region of interest for "V^ and 1^ is of course restricted to NR({ f{ }), where the 
functions { fa } satisfy definition (l). 1^ differs from Vfc in that on "Vfc the values 
(levels) of the functions fi are equal, whereas on !&, they are not. Furthermore, on 
"Vjt the value varies, whereas on J^ the values are fixed. 

Jfc may be expressed as 

i 

The claim is that J^ is homotopically equivalent to "14, that is, that if g : E — > 7 
is a chart for J^ and g' : E k — ► 7 is a chart for "V^, then there exists a continuous 
homotopy deformation h : E X I 1 — >• / between g and g' such that 
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h(Y,0) = g(Y) 
h(Y,l) = g'(Y). 

As usual, /' denotes the unit interval [0, 1]. For a review of elementary 
homotopy theory consult appendix II. 

The charts g and g' exist, since ifc and ~Vk are manifolds. (Assume without loss 
of generality that only one chart is required). A level C-manifold fj (£) (for some 
level £) is a 5-dimensional manifold and hence there exists a chart C,- : E 5 — » 7 for 
fT W* We demonstrate such charts in the proof of claim (I.i), below. 

Let £y be any achievable value for the functions /j along the Voronoi manifold 
"Vjt, that is, any £y such that there exists some X £ "V/t satisfying definition (1) for 
which 

/i(X) = ... = f n (X) = £y. 

Now, t(-i + (I — t)£y is a linear combination of the levels £,• and ty for /;. Since each- 
level C-surface 



fr^t£ i + (l-t)ty} 



is a manifold, each has a chart of the form C{, above. If t £ [0, 1], these are 
C-surfaces for /,• with level £ £ [i-y>A']> an< i their charts may be parameterized 
by t. Suppose we have a set of several level C-surfaces (as in Ik). Their chart3 
may be intersected to form a new chart for the intersection manifold. We define 
h : E X 1 1 — » 7 to be the chart for the intersection manifold at t, such that 



h(E k ,t) = f]fr^tii + (l-t)iyy 



We call h a chart family for the intersection manifold. 

Claim (LI): The chart family h can be constructed such that h(Y , t) is continuous 
in Y and t, within the area of interest for Vk and !&. (For proof, see below). 
'Sec the proof of claim (1.1), whore he is such a chart. 
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Recall that £{ is the value (or level) of /,- on the intersection manifold I k . Then 
h : E k X I X — ► 7 is a homotopy between g and g', that is, h continuously deforms 
Ik into "14(^"v)> where we use "14(^1* ) C Vjt to denote the Voronoi manifold restricted 
to level £y, i.e., 

V k {£y) = {X\ /,(X) = . • . = f n (X) = l v }c7. 
Verify that 

and that 

/»(js*,o) = n/r l ('v) 

We have shown that Ik c^h Vk(£y) {—h denotes homotopic equivalence) for all 
achievable ly. We must now show that Vk(£y) —h *V*(^V + e )- 

We are interested in continuous deformation within 7. Hence "Vk may be 
multiply connected within 7 , so long as it does not wrap around obstacles. Vk must 
be contractible to a point (within /). This is guaranteed by the construction of A 
(definition (l)) and 14, i.e., by the choice and domains of the functions /;. To see 
this, consider that if Vk did wrap around an obstacle in C-Space, then the value of 
some /i would have to go negative. Hence, it would become redundant, and could 
not be used in the construction of "Vfc. Note that the C- Voronoi diagram, which is 
the union of Voronoi manifolds such as Vjfc, will, in general, wrap around obstacles 
and be multiply connected. 

Furthermore, we can choose e such that the topology of "V^ does not change 
too drastically between Vki^v) an d Vk{£"\) + e )- (This is possible since Vk is 
finite-branching). So 
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I k ~ h V k (l v ) ~,, V k {l V + £,) ~ fc Vfc(£ V + *2) -fc ' • • -A W + «r). 

What we have shown is that J/t is homotopically equivalent to the "easy" parts 
of "Vfc (where the level of the Voronoi manifold is constant). We next showed that 
becaiise the topology of 14 is simple, we can paste together these restricted Voronoi 
manifolds. | 

Proof of Claim 1.1: The existence of a continuous chart family for the intersection 
manifold is based on our knowledge that the manifolds exist at certain levels, and 
from our ability to demonstrate such a chart for the intersection manifold. In 
chapter 4, we exhibited C-functions of six variables for the C-Space 3J X 50(3): 

fi :(x,y,z,i/>,e,4>) ->9£ 

which are continuous, affine in x, y, and z, and multilinear in the sines and cosines 
of the angles ip, 9, and <f>. The Linear Form for a C-function / t - : 9i ,} X 50(3) — * 9J, 
is an equivalent expression 

fi{x, y, z, 6) = Ei{e)x + E 2 {Q)y + E 3 {Q)z + £ 4 (9), (6.1) 

where Ej : 50(3) -> 3? (for j = 1, 2, 3, 4). Now, 

/r'(^ker(7(*,y,z,^M)-^. 
A chart for fj{t) may be found by solving 

fi(x, y, z, V>, 6,4)- I = 
for x (or y, or z) in terms of the other variables and I: 
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E 2 y + E 3 z + Ei-l 

x = . 

E X 

(We have dropped the 0, since the functions Ej are constant with respect to x, y, 
z, and I). If E\ = 0, then the solution for y or z may be employed. This yields the 
obvious chart 



C : E 5 -► 5ft 3 X SO(3) 
(y, z, V>, 6, (/>) »-»• ( - , y, z, if), 0, (j>) 

which we presented in chapter 4. C is affine in I, and can be used to construct a 
family of charts 



h c : (r, I) - 5R 3 X 50(3) 



for the C-surfaces 



/r 1 w = /r 1 (^.- + (i-*)^). 



which is continuous in Y = (y,z,ip,0,(p) and £ /*c is clearly a homotopy between 
level C- manifolds for /,-. 

Chapter 4 also derives charts for f7\ii)r\fj\lj), fT^WfJ^nfk^k), 
(and so on) by solving the C-functions simultaneously for the intersection manifold. 
For example, a chart for the intersection manifold of degree three 

1=1,2,3 

can be constructed by solving three simultaneous equations with the form of (6.1). 
For arbitrary coefficients Ej and levels £{, this intersection may not always exist. 
However, we know a priori that it exists for the specified levels £,' an< i ^V- From 
the form of (6.1), it is clear that if the intersection manifold exists for some levels 
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£{ (and if the coefficient functions Ej are independent), then it will exist for all 
levels. We omit a discussion of intersection manifolds of higher degree: the reader 
is referred to chapter 4 for further details. | 

Corollary: For every Voronoi manifold V;, there exists a strongly equivalent 
intersection manifold I{. 

Corollary: If F{ is the set of constraints used to construct "V;, l et NR(Fi) denote 
the region in free-space where all of the constraints in F{ satisfy definition (l). If 
X' exists, then in every connected component of NR(Fi), there exists a strongly 
equivalent intersection manifold I; built out of Fi. 

Proof of corollaries: All / <E F { exist within NR(F { ). Pick any X e NR{Fi). 
Evaluate all the functions in F{ at X to obtain a set of levels. The intersection 
manifold must exist at these levels, since we have demonstrated that X is on the 
intersection manifold. The intersection manifold from the C-surfaces at these levels 
is by definition and by theorem (I) strongly equivalent to "V t \ I 

Next, observe that for all 1 < i < k, either X' C Vi+i or V l+ i C V{. In other 
words, to move from V{ to "Vj'+l> we either add or remove one or more constraints: 

1/,- = {X | /!(*) = ••• =/*,} 

and either ki > A;; +1 or k{ < k{ + \. We call ki and fc t+ i the degree of the Voronoi 
manifolds. 

We have shown that for a Voronoi chain "Vj, . . . , "Vfc, a sequence of intersection 
manifolds It,..., Jjt may be constructed such that each 1{ is strongly equivalent 
to Vj (for 1 < i < k). However, the sequence of intersection manifolds may 
be disconnected. We now furnish a theorem demonstrating that the intersection 
manifolds may be constructed in such a manner that they can be connected together 
by a series of special intersection manifolds, called bridges. 

Theorem II: The intersection manifolds I\, I2, . . . , Ik may be constructed such 
that each pair of intersection manifolds J t - and Jj + i can be connected by a sequence 
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of "bridges." Each bridge is an intersection manifold equivalent (but not strongly 
equivalent) to V;. 

Proof: Let I\ be a strongly equivalent intersection manifold to "Vi, constructed 
with C-functions F\. Along 1\, all constraints in Fi are non-redundant. Let V 2 
be the next Voronoi manifold after "Vi in the Voronoi chain, and let F 2 be the 
constraints constructing ~\) 2 . 

Case (i): If F 2 C Fi, then "Vi is lower in dimension that V 2 - We can construct 
I 2 , a strongly equivalent intersection manifold to V 2 , which is connected with I\, 
be removing one or more constraints in F\. (We remove exactly the constraints 
Fi - F 2 ). This is possible because if F 2 C F lt then JVfl(Fi) C NR(F 2 ): 



fj€Fi 

h= n fj 1 ^). (6.2) 

Strictly speaking, equation (6.2) should employ the subset notation (C) instead of 
equality (=), since Ii and I 2 are restricted to where the intersection is applicable and 
non-redundant. However, the equality makes the construction more transparent. 
Note that the construction still works with the subset notation, since NR(Fi) is a 
subset of NR(F 2 ). Since F 2 C F\, I\ and I 2 agree on the levels for C-functions in 
F 2 . Since Ji C I 2 > Ii an d I 2 are connected. 

Case (ii-a): Suppose, however, that F± C F 2 . Then V 2 is lower in dimension 
than 1/i, and NR{F 2 ) C NR(Fi). 

We know that I x C NR(F { ). If I { f\NR(F 2 ) 7^ 0, then we can construct J 2 
from F 2 such that I 2 C NR(F 2 ), J 2 C I\, and in addition, I\ and J2 agree on the 
levels in F{. Construct 1 2 as follows: pick a point Xq G I\ r\NR{F 2 ). Evaluate each 
fj € F 2 — F\ at Xq, to obtain a level cy = fj(Xo). Construct: 



j a = Ji n( n fj\cA 
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'A/R ( Ift 



Figure 6.3. Case where I, f| NR(F 2 ) ^ 0. 



We showed earlier that I2 may be constructed in this manner. 

Case (ii-b): The hard case is when IiftNRffi) — 0. In this case, we must 
construct some strongly equivalent (to V2) intersection manifold I2 C NR(F2) with 
different levels from 1\ with respect to the C-functions F\. We then build a sequence 
of bridge manifolds, entirely within NR{F{), between I\ and I2, connecting them 
together. 



The bridge intersection manifolds are constructed out of some subset Fq C F\, 
and each bridge manifold is equivalent (but not strongly equivalent) to Vi- The 
bridge manifolds are formed by relaxing one or more constraints in Fi to be able 
to move from NR{F{) — NR(F 2 ) into NR(F 2 ). The motions slide along intersection 
manifolds constructed from the remaining constraints. Once inside NR(F2), we 
construct I2 there, and I2 is strongly equivalent to 1*2 . Note the levels at which I2 
is constructed, (with respect to the constraints in Fi), arc typically different from 
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N^O 



Figure 6.4. Case where I t f| NR(Fz) — 0. 

the levels at which Ji is constructed. The existence of bridge manifolds is covered 

in theorem III. | 

We will use the dot notation for the Riemannian inner product on the tangent 
space at X. The normal to a level C-surface f~ l (t) is V/, and depends on the 
inner product. We assume throughout that the normals are unit vectors. A basic 
concept in these proofs is that of general position, or transversality (see Hirsch, 
1976). Two submanifolds M , N of a manifold V are in general position if at every 
point of Mf\N the tangent spaces of M and N span that of V. If A and B are 
not in general position, then arbitrarily small perturbations of one of them will put 
them in general position. In our case, M and N correspond to level C-surfaces and 
their intersection manifolds, and V to 3? 3 X 50(3). The proofs still work even if 
M and N are in general position only at "many" points of M f\ N. We will write 
the condition of general position for two level C-surfaces / _1 (^i) and g~ (£2) as 
VfVg^l. 
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Theorem HI The Existence of Bridge Manifolds: (Bridges of dimension five). 
Let S be a path-connected component of NR(Fi), X ,Xi € S, and f,g£F\. Note 
that S lies with in the domain of / and g, and that by construction, i(S) is an 
open set. Then if V/ • V# 7^ 1 within S, then there exists an infinite sequence of 
five dimensional bridge manifolds within S, connecting Xo and X\. 

Proof: Each bridge manifold will be of the form f~ l {c/) or g~ l (c g ), for different 
levels of Cf and c g . Note that as we vary cj, f~~ i {c/) covers 5 (similarly for g). 
Let Tx denote the six dimensional tangent space at X. If cq = f{X), the level 
C-surface / _1 (co) is a five dimensional submanifold of 5ft 3 X SO(3), with a five 
dimensional tangent space, T x . That is, identifying T x with a subspace of Tx, 

T f x = {v<ETx\v.Vf(X) = 0}. 

It is easy to show that T x \JT X spans T x , for all X where V/(X) • Vg(X) ^ 1. 
Let N x denote the normal space at X with respect to /, such that 

N x = {v£Tx\v = aVf(X)}, 

for all scalars a € fit. So T x = N f x ® T f x - Clearly, if V/(X) • Vg(X) ^ 1, then 
Nl is spanned by T x U T x .. Hence T x = T f x + T x - 

Since the space of differentially tangent directions to the two level C-surfaces 
at X is equal to the space of all directions, there exists an infinite sequence of 
differential moves along level C-surfaces for / and g, at different levels, to realize 
any path within S. Since S is connected, there exists such a path from Xo to Xi- 
D 

Corollary I1I.1: (Bridges of dimension four). A direct result is the existence 
of a sequence of bridges which are four dimensional intersection manifolds. Let f\, 
h, h, h e Fi. Suppose that within S, V/;(X) • V/y(X) ^ 1 (for i ^ j). Then 
there exists an infinite sequence of bridges between X<> and Xi , where each bridge 
is of the form 
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ju^if 4 



*•*•!* 



Figure 6.5. A finite path iterating along levels of / and levels of g. 



MhJ) = f7 1 (a)rif7 l ('j)- 



(» 7^ 3) 



This is reasonable, since each Jb(«*,;) is a four dimensional manifold. If the 
normals are all independent at X, then the direct sum of the tangent spaces to all 
possible intersection manifolds Ijj{hj) 1S clearly Tx' 



r Jf «(r$nr§) + (r$nr$) + ---+(r$nr§) + "-+(ri»nr§). 

(Of course, i =^ j for all terms in this sum). | 

Corollary III. 2: (Existence of a finite sequence of bridges). We now argue that if 
there exists an infinite sequence of bridges from X to X\ within S, then there also 
exists a finite sequence. 
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Informally, we argue that it is always possible to to move a certain distance e 
along each level C-surface, and that this e cannot grow arbitrarily small. First of 
all, note that S is not infinitesimal, and that i(S) is an open set. (If it were not, 
it might be necessary to make an infinite number of differential motions to remain 
within S). 

We also appeal to the well-behaved structure of the level C-surfaces, and their 
intersection manifolds. The level C-surfaces are smooth, with normals that change 
continuously. (If the normals changed discontinuously, we might not be able to 
take finite steps). Thus we can move a finite (i.e., not infinitessimal) distance along 
the surfaces to a point where the normals are still independent, and where the 
surfaces are "similar" (i.e., having normals in almost the same direction as before). 
Furthermore, for any two levels of / within S, there exists a homotopy between 
them. These continuity arguments indicate that it should be possible to move in 
finite steps along the intersection manifolds, and hence we can reach X\ from Xq 
in a finite number of bridges. 

Suppose from Xq to X\ there exists an infinite sequence of bridge manifolds, 
but no finite sequence. Then either (l) %{S) is not an open set (and therefore 
only differential motions can stay within it), or (2) for a subset PCS, whose 
cardinality is that of the continuum, the entire tangent space is not available along 
the level C-surfaces. In both cases, our initial hypotheses are violated. (1) violates 
the assumption i(S) is an open set, and (2) the assumption of general position. 

We formalize this argument as follows: 

Definition: Let U be a metric space, and p, p' : J 1 -+ U be paths in U. Let { U a } 
be an open cover of p(I l ) in U, where each U a is a neighborhood of radius < r, 
and U a C)p{I X ) 7^ $• We say that p' approximates p at resolution r if { U a } is an 
open cover for p'(I^) also; that is, if p'(I l ) C U« U a . 

Claim (III. 2.1) shows that an arbitrary curve in some neighborhood U of 
free-space can be approximated by a path within U along a finite sequence of 
intersection manifolds. The proof of Cor. (III. 2) then employs the fact that the curve 
is compact, and therefore can be covered by a finite number of such neighborhoods. 
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f-'C<=."l 



f-'<^ 



Figure 6.6. Let cq = f(X ) and Ci = f(Xi). X lies on f~ 1 {co)r\g~ 1 (c ), and X t lies on 
/ -1 ( c i)ri3 _1 ( c i)- The path segment between Xq and Xy may be approximated by a path sliding 
first along g~ l {co) from X to X', and then from X' to Xj along f~ l {c{). 

Claim III. 2.1: Any path within a neighborhood where / and g are defined, may 
be approximated to an arbitrary resolution by a finite sequence of motions along 
level C-surfaces of / and g. 

Proof: We will regard level C-surfaces of / and g as trivial intersection manifolds. 
Consider how one can approximate a path from Xo to X\ by a path along 
intersection manifolds. Let ci be a metric on 9? 3 X SO(3), and /, g be C-functions 
with inverse images covering a neighborhood containing Xq and X\. Let en = /(Xo) 
and c\ = f(X{). Xq lies on the intersection manifold / -1 (co)ri3 _1 ( c o)j an( i -Xi ^ es 
on / _1 (ci)n? _1 ( c i)- Construct / _1 (ci)rifi' -l ( c o) (refer to fig. 6.6), and choose X' 
to be the any of the closest points to Xq on this manifold. We can construct a path 
which slides from Xn to X' along g -l (co), and then slides from X' to X\ along 
f~ 1 (ci). We wish to demonstrate that by choosing Xi sufficiently close to Xn, X' 
can be made to lie arbitrarily close to Xq; that is, for all e > 0, there exists a 



and Cq 1 = 



g(X Q ), c 1 l = g(X x ). 

2M 



6 > so that <1{Xq,X') < e whenever <f(Xo,Xi) < 8. This is definitional, since 
lim Xl -.x o r 1 (ci) = r 1 M- 

Thus for every neighborhood B t {Xq) of radius r about Xo, there exists an Xi £ 
B r (Xo) such that X' £ 2? r (Xo) also. Furthermore, a!(Xo,X') is finite (and non-zero). 
Of course, a similar argument holds for the path segment between Xi and X'. Thus 
any path within a neighborhood where / and g are defined, may be approximated to 
an arbitrary resolution by a finite sequence of motions along intersection manifolds. 
I 

Clearly, similar arguments hold for intersection manifolds of higher degree. 

Proof (Corollary III. 2): (Sketch) Let p(/ x ) be a path within i(S) from X to Xi 
along an infinite sequence of intersection manifolds. In short, p(I x ) C i[S). Choose 
an open cover { U a }, relative to 3? 3 X 50(3), for p(/ ! ) such that U a U a C i(S). 
p(l ) is compact, hence there exists a finite subcover, i.e., for finitely many indices 
a\, . . ., a n , we have 

Now, for each U ai , we can construct a path p'(/*) along a finite sequence of 
intersection manifolds approximating p(I l )f]U ai (Claim III.2.1). That is, p'(I l ) is 
also contained within the closure of U ai . Furthermore, it is not hard to construct 
p' such that it leaves the neighborhood U a . at the same points as p, that is, so 
that p{I l )f]dU a . = p'{I x )C[dU ai . Since this is true for all U ai , then p(I x ) can be 
approximated by some path contained within U ai \J • • -\J U a „, and which lies on 
some finite chain of intersection manifolds. | 

Theorem (IV) is almost immediate, and its proof similar to that of Theorem 
(I)- 

Theorem IV: For every connected chain of Voronoi manifolds Cy = Vi, . . . , 14, 
there exists an equivalent (in the sense of theorems (I — III)) connected chain of 
intersection manifolds Cj = Ii, . . . , Ik> such that the entire Voronoi chain Cy is 
homotopically equivalent to the intersection chain Cj. That is, 
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U T*=*k U Iy 

Vi£Cv Ij€Ci 

Proof: Simply apply the fact that each equivalent pair ("V,J) (where V is a 
Voronoi manifold and I is an equivalent intersection manifold) must lie in the same 
non-redundancy region. Hence if one chain wraps around an obstacle, so must the 
other; furthermore, each chain must wrap around the same obstacles. I 

Future Research 

There are several interesting problems which are left open. They include the 
following: 

i) We have demonstrated an equivalent chain of intersection manifolds for any 
connected chain of Voronoi manifolds. Show whether or not a strongly equivalent 
chain exists also. 

ii) Show whether or not for every Voronoi chain Cy there exists a (strongly) 
equivalent intersection chain exhibiting a bijective correspondence to Cy. 

iii) Devise an effective procedure for constructing a chain of intersection manifolds 
to realize any class of paths in free space. 

iv) Derive complexity bounds on the construction of the C- Voronoi diagram and 
the equivalent intersection chains. 

v) Other configuration spaces, such as those arising in the hinged body problem, 
should be considered. 

vi) To extend these results to configuration spaces generated by real space 
constraints which are not polyhedral (for example, algebraic surfaces), a 
generalization of the GVD such as smoothed local symmetries (Brady, 1982b) 
could be considered. 
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7 
Conclusion 



In chapters 1 through 6, we developed representations and algorithms for 
automated spatial planning with six degrees of freedom. To demonstrate the 
competence of the representation and the feasibility of the algorithms, a planning 
system for the classical find-path problem with six degrees of freedom was 
implemented. The planner is of considerable intrinsic interest, in that it is complete 
(for a given resolution). Experiments have demonstrated that this algorithm can 
solve find-path problems requiring six degree of freedom solutions that were beyond 
the competence of earlier, approximate planners. The mathematical framework 
developed here impacts a class of geometric planning problems for three dimensional 
objects. 

The planning algorithm may be explained by analogy with the Point Navigation 
Operators. The C- Space transformation reduced the motion planning problem to 
the task of navigating a point in 3J' 5 X SO{Z). Since the path for the point must 
avoid the C-Space obstacles, which are curved, six dimensional manifolds with 
boundary, clearly paths can be found in C-Space by the closure of three operators: 

(i) slides along 1- to 4- dimensional intersections of level C-surfaces; 
(ii) slides along 5-dimensional level C-surfaces; 
(iii) jumps between 6-dimensional obstacles. 
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However, these simple operators could not be implemented until a series 
of representational and algorithmic questions were solved. The fundamental 
representational issues centered on the structural properties of the defining C- 
functions. By deriving their domains, and by proving theorems about the topology 
of these domains, it was then possible to address the intersection problem for 
high-dimensional configuration spaces. By solving these open problems, developing 
representations for the C-functions and their domains, and designing decomposition 
algorithms in C-Space, it became possible to represent the constraints on motion 
"completely," and to exploit the complete representation in implementing a planning 
algorithm. Next, we presented new theoretical results on the C-Voronoi diagram. 
By showing that for every connected chain of Voronoi manifolds, there exists 
an equivalent chain of intersection manifolds of level C-surfaces, a theoretical 
completeness result for planning along the intersection manifolds was obtained. 
This result is also of interest since while the charts for the Voronoi manifolds are as 
yet unknown, charts for the intersection manifolds are straightforward (given our 
representational framework). Thus it is possible, in principle, to devise a planning 
algorithm with all the advantages of a planner along the CVD. 

There is much work to be done. Ultimately, decomposition algorithms such as 
those we present in chapter 5 will become increasingly important in partitioning 
C-Space into regions where the set of applicable (or alternatively, relevant) 
constraints is invariant (see also Schwartz and Sharir (1981)). The representations 
and algorithms we have developed may make other geometric planning problems — 
such as fine-motion, and planning with uncertainty — more feasible, and should now 
be applied in these applications. The find-path algorithm can be easily extended 
to robot manipulators with six degrees of freedom in which translations can be 
decoupled from rotations. This class includes Cartesian manipulators (for example, 
the IBM RS/1). The adaptation of this work to a production environment presents 
interesting engineering challenges. 

In principle, the 6DOF representations can be extended to revolutc-joint, linked 
arms with six degrees of freedom. However, the C-Space of the linked-arm problem 
is the six dimensional torus, 
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S l X • • • X S 1 (to 6) 

which has a very different structure from 5ft 3 X SO(3). It is our hope that this thesis 
can present a methodology for formulating the geometric constraints for arbitrary 
configuration spaces, and that a similar structure will be found for constraints on 
the 6-torus. 

Our planning algorithm is complete (at a resolution), in that the representation 
employed is complete, and in that the search is guaranteed to find a path if one 
exists at that resolution. However, since it is a search algorithm, we cannot provide a 
polynomial time bound. Our motivation has been to address the completeness issue 
first, by resolving fundamental representational questions; now, one of the most 
important remaining tasks is to develop complete, polynomial- time algorithms which 
can actually be implemented. We do not believe that the worst-case exponential 
behavior of the 6DOF planner is inherent in the representation, and conjecture 
that a polynomial-time algorithm which plans paths along intersection manifolds 
can be devised. Indeed, the theoretical results on the CVD are suggestive that the 
limiting complexity of the approach may be the complexity of constructing the 
CVD or an equivalent chain of intersection manifolds. More research is needed on 
the topology of the CVD. A fast planning system might determine what constraints 
construct the CVD, and, using these constraints, construct a chain of intersection 
manifolds which could attain the goal. The first step in this effort would bound the 
complexity of the CVD and the intersection chains. 
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Appendix I 
Details of the Intersection Problem, and Related Problems 

This appendix contains the detailed equations from chapter 4, which are 
relegated to an appendix because of their excessive length. Some definition of terms 
is repeated, so that the interested reader will not have to flip back and forth too 
much. 

1.1. Intersecting Two C-surfaces in 9J 2 X S 1 

By applying trigonometric reductions we can express type (a) and (b) constraints 
as follows (only (al) and (bl) are shown): 

cos(\)Sy + Csin(X t -)y — sin(X t )5x + Ccos(X t )x 
+ sin(X; - 7y)|IM|S - IKII cos ( X *' _ Vi) 
-Ccos(X,--7y)IIMI ( al ) 



sm(<l>j)y + cos(4>j)x - ||a,-|| sin(^y - rn)S 

- C||o,-|| cos(^y - rn) - ||6y|| cos(<^ - 7y) (61) 



Where 



C = cos and S — sin 0. 



Now, we can consider a pair of these equations as a system in four variables, 
(x,y,C,S), and proceed to solve (al) and (b2), (hi) and (b2), and (al) and (bl) for 
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x and y. For example, the intersection of two type (a) surfaces, (al) and (a2) is a 
curve 

P : r„, f>«2 - 3* 2 X S 1 

where r a \ (~}r a <2 C S 1 denotes the intersected applicability constraints for (al) and 
(a2). Although the solutions are in the variables C and S, we can use C — cos 6 = 
cos r and S — sin — sin r to generate the curve of intersection in 3? X S . 

After much simplification, the parametric solutions for the intersection curves 

for type (a) and (b) constraints in 3J 2 X S 1 are as follows: 

The intersection of two type (a) Surfaces: (al) fl(a2) 

Po{r) = r, 

p x (r) = Z>(-(S(||afl| cos^/ + X,- - X/) + ||a/|| cos(r,l - \ { - X/) 

- ||a t -|| cos(X t - + r)i - \) - \\ai\\ cos(-X t - +m~ \) 

+ C(2cos(X? + X,. - 7 ;)||6;|| - 2cos(X/ + X,- - 7 y)IIMI)) 
+ C(\\a<\\ sin(^ + X,. - X,0 - ||a?|| sulfa/ - X,- - Xfl 

- ||a,-|| sin(\; + m ~ X t ) + ||a;|| sin(-X/ + Vi - \)) 

+ C\2 sin(Xi + X t - - 7 ;)l|6yll - 2 ain(X« + X; - 7y )||6y||) 
+ (- sin(X/ + X,. - 7 j) - sin(Xi - X; - 7 ;.))ll*yll 

+ (Bin(X* + X,- - 7 y) - Bin(X? - X,- + 7y))IND/ 2 )> 
Py(r) = ^(-(5(||a/|| sulfa/ + X,- - X?) - ||a?|| sinfa< - X,- - X/) 

- ||a;|| sin(X/ + rji ~ \) + \\ a i\\ sin(-X/ + % - X,-) 

+ C(2sin(X? + X,- - 7 ;.)||6;-|| - 2sin(X/ + \ - 7 y)IMI)) 
+ C(-||o/|| cosfa? + X t - - X?) - ||a<|| cosfa/ - X,- - X*) 
+ ||a t -|| cos(X/ + rji ~ \) + ||a;|| cos(-X/ + m ~ \)) 
+ C 2 (2 cos(X/ + X,. - 7y )||6 y || - 2 cos(Xi + X,- - 7J)||^II) 

+ (cos(x? + x,. - 7 ;.j - cos(x? - x,. - 7 ;.))||6;n 

+ (cos(X t ? - X t - + 1j) ~ cos(X/ + \i - 7;))|IMI)/ 2 )> 



where 



D = csc(X/ - X t ). 
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The Intersection of type (a) and (b) Surfaces: (al)f\(b2) 

p [r) = r, 

p x (r) = F(s{2C\\a!\\ cos(# - r)\ - X t ) 

+ ||6;|| cos(^. + X, - 7 j) - INI cos(^- + X,- - 7 y) 

+ n&;.|| cos(^- - x t - - 7 j) + HMI cos W - X i + 7,')) 

+ llo/H sin(^. - nl + X,-) - 2C 2 |K'|| sin(# - vi ~ \) 
+ \\a-\\ sin(f- - rjl - \) - |KII sin(<£j + r} { - \) 
-HoiHsin^y-j/i + X,-) 
+ C(||6j|| sin(# + X,- - 7J) - ||6y|| Bin(# + X,- - 7j ) 

- ||fr<|| sin(# - X,- - 7fl " INI sin(# - X,. + 7,-))), 

Pv(r) = -F(s{2C\\al\\ sin(# - ^ - X,-) 

- ||6j|| sin(# + \ - 1'j) + \\bj\\ sin(# + X t - - lj) 
+ ||&;.|| sinfoj - X,- - 7 y) - IIM sin(# - X,- + 7,0) 

+ ||a/|| cos(«/»;. - rjl + \f) + 2C 2 \\a{\\ cos(# - V < - X,-) 

- || a/ 1| cos(</>j - r)\ - \) - IKH cos(^} + w - X;) 
-||oi||cos(^--»7i + Xi) 

+ C(||6j|| cos(# + \ { - 7y) - INI cos(# + X; - 7,0 

+ ||&;|| cos(# - X,- - 7y) - INI cos(# - X,- + 7,0)), 
where 



F = 



(2 cos(^- - X t -)5 - 2C sin^j- - X t -)) 
csc(0 - 4>) + x,0 
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The Intersection of two type (b) Surfaces: (61) 0(b2) 

Po(r) = r, 

p x (r) = £((S(|K|| cosfa; + 6j - v i) - \\a { \\ cos(^ + fy - Vi) 

- \\a\\\ cos($ - fy - Vi) + IKII cos(^- - 4>j + Vi)) 

+ C(-||a/|| sin(<^ + fy - Vi) + ||a ( -|| sin^j + fy - Vi) 
+ ||aj|| sin(# - fy - ril) + ||a t -|| sin(# - fy + Vi)) 

- ||&;|| sin(# + +,■ - 7 j.) + ||6y|| sin(# + ft - 7y ) 

+ ||6<|| 8in(# - ^ - 7 y) + IIMI sinfoj - *y + 7 y))/2), 

PvM = ^(IK'H sin(^- + 0y - ^) - |k|| sin(# + fy - Vi) 
+ ||a/|| sin(# - 4>j - Vi) + IHI sin(^- - fy + Vi)) 
+ C(||a/|| cos(<^- + <j>j - rj'i) - \\ai\\ cos(<£J + <f>j - Vi) 
+ || a- 1| cos(</.y - </>j - ?7 t ') - 1 1 a,- 1 1 cos(</>j - <£y + Vi)) 
+ ll&jll cos(^- + </>j - 7y) - IIMI cos(# + 0y - 7 y) 
+ ||6<|| cob(# - fy - 7 j) - ||6y|| cos(^. - ft + 7 y))/2), 






i? = csc(</>j — 0y). 
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1.2. Related Problems in 5ft 2 X S l 

1.2.1. Techniques for Moving Along C-Surfaces in 5ft 2 X S l 

In this section we present techniques for moving along a C-Surface. We could 
imagine using these methods to move to the nearest "edge" (C-Surface intersection), 
for example. A level C-Surface is defined via a function f(x, y,0) = k for k constant. 
/ is exactly of form (al) or (6.1) (above), and the level surface in 5ft 2 X S l is all 
points 

L={Xe® 2 Xr f \f(X) = k}, 

where rj C S x is the 6 applicability range for /. 

Define a hyperplane in 5ft 2 X S 1 as the set 

p = {X e 5ft 2 X s l | X ■ H = -h A }, 

where H — (hi, h 2) /13). 

We intersect the level surface Lwith the hyperplane P to obtain the intersection 
curve: 

Po{t) = r, 

Px(t) = Gi(s(cos(X t -)(-M - h A ) + h 2 sin(X t - - 7 ;)IMI) 
+ Csin(X;)(-h 3 i - h A ) — hi\\ai\\ cos(X; — rji) 

- Ch 2 cos(X l - - lj)\\bj\\ - h 2 k\ 

p y (t) = -G,(s(sin(Xi)(M + h A ) + h x sin(X; - 7j)IIMI) 
+ Ccos(X,-)(— h-$t — h A ) — h\\\ai\\ cos(X,- — rn) 

- Chi cos(X i - 7i )||6 y || - hikj, (PPKol)) 



where 



G = I . 

(h 2 sin(Xj) + hi cos(Xi))£ + (h\ sin(Xj) — h 2 cos(Xj))(7 
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where 



Po{t) = r, 

Px[t) = -G 2 ( sin(^y)(M + /14) + MKII sin (^i - ^i') 5 
+ C/i 2 1 1 «; 1 1 cos(<£y- 77,-) 

+ /l2 1 1 fej || COs(<£y — 7y) + /i2& J, 

p y (f) = G 2 ( cos(^-)(/i 3 i + ^ 4 ) + MIKil sin (^y - ^i) 5 

+ Chi\\a { \\cos(<f>j - rji) 

+ hi\\bj\\ cos(^- - 7 y) + /*!*), (^D(61)) 



G 2 = 



hi sin(^y) — /i 2 cos(<^y) 



1.2.2. Characterizing Clearance to a C-Surface 

It would be very useful to characterize the minimum clearance to a C-surface. 
We would like to answer the question: 

• For a point b xy £ 9J ; at what orientation is b xy closest to a C-surface, and 
what is minimum directed clearance vector at that orientation? 

Using Lagrange multipliers, we can minimize a function f(x, y, 0) subject to a 
constraint g(x, y, 0) = by constructing the auxiliary function 

H(x, y, 0, £) = f(x, y, 0) - lg(x, y, 0) 

and solving the partial derivatives 

^ = CD 

ox 

f- - (2, 

dy 

%-*. «. 

In our case, g will define a C-surface, for example, a type (a) surface: 
g(x, y, 0) = sin(0 + \)y + cos(0 + X;)x - ||6y|| cos(<? + \ - 7y) - ||a t -|| cos(X t - - rn) 
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and / will be a distance function. Now, the rotational dimensions cannot be treated 
uniformly in establishing a metric, so we will define distance in Euclidean space. 
Minimizing the square of the translational distance suffices for our purposes. Hence, 

f{x,y,6) = {x-b x ) 2 + {y-b y ) 2 . 

Differentiating II gives us the following equations: 
air 

-— = 2{x-b x )-lcos{9 + \i) (1) 

ox 

— = 2(y-b y )-l a m{0 + \i) (2) 
oy 

BFf 

— = -^(cos(0 + \i)y - sin(0 + X;)x + ||6y|| sin(0 + X,- - 7,-)) (3) 
off 

f)JT 

— = (- sin(0 + X t -)y - cos(0 + X;)x + ||6y|| cos(0 + X; - 7y) 

+ IH|cos(t7 1 --X f )) (4) 

Solving these equations for x, y, 0, and £ is not trivial. However, the following 
observations make it easier. First of all, we note that solving 

l™= (5) 

at K ' 

is equivalent to solving (4) as long as £ = is not a solution. We next solve (l) and 
(2) for ^cos(0 + X-j) and £s'm(0 + \) and substititute this value in to (3) and (5). 
(5) then becomes a linear equation in x and y while (3) is quadratic in x and y and 
linear in £. Our rewriting of (4) into (5) has thus eliminated £ from (5), and we can 
solve for x in terms of y: 

-2y 2 + (2||6y|| sin( 7 y) + 2b y )y - 2x 2 + (2||6y|| cosf/yy) + 2b x )x 

+ £\\ai\\ cosfa - \) - 2b y \\bj\\ sin( 7 y) - 2b x \\bj\\ cos( 7i ) (3) 

(2b x - 2\\bj\\ cos(7y))y + (2||6,-|| sin( 7 y) - 26 y )x 

- 2b x \\bj\\ sin( 7 y) + 26 y ||6,-|| cos( 7j ) (5) 

We need one additional constraint: this is obtained by observing that 

sin(0 + X;) 2 + cos(0 -f X,-) 2 = 1. (6) 

Since the trigonometric terms can be expressed in x, y, and £, we can obtain £ in 
terms of x and y. (3), (5), and (6) then result in a quartic in x with the roots: 
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X = GyUb y \\bjf sin(2 7 y) + 26 I ||fey|| 2 cos(2 7y ) 

- ((2&J + 26j)||6y|| + 2||6 J || 3 )cos( 7i ) + 2b x \\b j f 
±G 2 (||a t -||||6 i ||coB(»7 I --X t - + 7y) 

+ ||a. t ||||fej|| 003(77,- - \ - lj) ~ 26 X |K|| cos im ~ K)j J, 



where 



Gi = 



2(26 s ||6y|| sin( 7 y) + 26.||6y|| cos( 7y ) - \\bj\\* - b' y - 6») 



and 



G 2 = V C 2M^-||.8in( 7 y) - 26,||6y|| cos( 7y ) + \\b^ + b\ + b%. 
Given x, y is found from (5) : 

|6y|| sin( 7y ) - by)x - b x \\bj\\ sin( 7 y) + &„||iy|| cos( 7j -) 



y = 



bj\\ cos{n j)-b* 



t can be found from (3) as a linear function of x and y. To determine 0, we calculate 
sin(0 + X,) and cos(0 + X,-) from 



cos(# + Xj) 



2{x - b x ) 



and use a two argument arctangent function Atan2 : 3i — >■ S to determine 9 + \. 
The value must be checked against the applicability constraints for surface g; 
if it falls outside the range, then endpoints will yield the minimum clearance. 
Naturally, it is possible that for certain orientations, (b X) b y ,0) will lie on or inside 
the C-surface. These cases may be disambiguated by the sign of g(b x , b y , 0). Finally, 
given the closest point (at some orientation 0) on the C-Surface, the minimum 
clearance is simply the vector 

(x,y)-(b X! b y ). 
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The Minimum Clearance to a type (b) C-surface 

To find the minimum clearance to a type (b) surface and the orientation at 
which the clearance occurs, we let g be a type (b) constraint (equation (bl)) and 
solve the system of partial derivatives of H. 

BJ-f 

= 2{x-b x )-lcos{<j> j ) (1) 

^ = 2(y-b y )-lsin(<j> j ) (2) 

^E. = -^||a I -||8in(tf-^ + i 7 (3) 

™=-g{x,y,0) (4) 

The solution is considerably easier because the form of the constraint surface is 
less complicated. Since dH/df, = —g, 6 may be found in terms of x and y using an 
arccosine. Substituting this value of 9 into (3) yields a quadratic equation in x, y, 
and I, which when solved with (l) and (2) for the following roots: 

x = \\bj\\ cos(<f>j) cos(<j>j - 7,) 

— by cos(<j>j) s'm(<j>j) — b x cos(^) 2 — ||a t || cos(^y) + b X) 
V = INI sin(^) cos(^- - 7,-) 

- (b x cos((j>j) ± ||a t -||) sin(^y) + b y cos(</>y) 2 , 

I = 2|j6y|| cos(^- - 7j -) - 2b y sin(^y) - 2b x cos(^y) - 2||a t ||. 
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Appendix IT 
The Connectivity of Configuration Space 

n.l. A Review of Elementary Homotopy Theory 

In this appendix we review some elementary homotopy theory, and address 
the connectivity of configuration space. See Hocking and Young (1961) for a more 
extensive review, and Donald (1983a) for an analysis of the relation between 
channels and homo topic equivalence classes. Let 7 1 denote the unit interval. A 
parameterized family of mappings from a space X into a space Y is a continuous 
function h : X X I 1 — ► Y. Consider the mappings / and g from X to Y: we say 
that h is a homotopy between / and g if for each point i in X, 

h(x, 0) = f(x) and h(x, 1) = g(x). 

Intuitively the existence of h implies that / can be continuously deformed into g 
without leaving Y. 

The homotopy relation between mappings from X into Y is an equivalence 
relation on the function space Y . Hence the homotopy relation partitions Y 
into disjoint equivalence classes, which are called homotopy classes. We write the 
homotopy relation as / ~ g. These homotopy classes capture our intuitive notion 
of classes of paths. The homotopy classes of Y x can be shown to be precisely the 
arcwise-connected components of Y x (Hocking and Young (1961) ). 
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To take a concrete example, consider configuration space for the two-dimensional 
mover's problem to be the product space of the 2-dimensional Euclidean plane 5ft 
and the one- dimensional sphere 5 1 to obtain 5ft 2 X S l , and denote the configuration 
obstacles as CO C 5ft 2 X S l . Now two paths / and g in the same equivalence class 
must belong to a parameterized family of mappings such that: 

h : X X P -> 5ft 2 X S 1 - CO. 

and h(x, 0) = /(a:), h(x, 1) = g(x) as before. 

Now, let y be a point in Y . The y neighborhood of cyclic paths in Y , C(Y , y) 
is the collection of all continuous mappings f : I 1 -+Y such that /(0) = /(l) = y, 
i.e., the set of all continuous curves that begin and end at y. If / and g are curves 
in C(Y,y), we say that / is homotopic to g modulo y if there exists a homotopy 
h : I L X I 1 — * Y continuously deforming / into g without leaving Y . 

Clearly, homotopy modulo y is an equivalence relation, and decomposes C(Y, y) 
into disjoint equivalence classes which are exactly the arcwise-connectcd components 
of C(Y, y). The set of these equivalence classes is termed the first homotopy group, 
or fundamental group of Y. We say a path-connected space Y is simply-connected 
if the fundamental homotopy group for X is the trivial group of one element 
(for some, and hence for all y in Y). See also appendix III, section "Topological 
Constraints." 

II.2. The Connectivity of Configuration Space 

The configuration space 3J 2 X S x (for the two-dimensional mover's problem) 
is not simply-connected, since S l is not simply-connected. The function space 
(9£ n X S l ) x contains several homotopy classes. (5ft 2 X S l ) x may be envisioned as 
a cylinder on which there are clearly two classes of paths: those that bound a 
2-dimensional region and are contractable to a point, and those that go around the 
cylinder. 

The configuration space 5ft 3 X SO(3) is not simply connected, because 50(3) 
is not simply connected. To see this consider the following: geometrically, 50(3) 
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is homeomorphic to P 3 , the 3-sphere with antipodal points identified. As is well 
known (see Massey (1967), p. 166) the fundamental group for P" is cyclic of order 
2, and hence P n is not simply connected. 

General configuration spaces (other than that for the classical Mover's problem) 
are not always simply-connected. For example, the C-Space for a manipulator with 
six revolute joints is the 6-torus, S 1 X S l X • • • X S l (to 6). 

Let II be the half-open interval [— 7r,7r). II can be used to approximate S , 
if we are willing to tolerate singularities in the representation. It is instructive to 
generate a configuration space which is simply-connected. Since this is not possible 
for the general product space 3J n X 5 1 we will instead consider the product space of 
3ft" and II. Thus for the two-dimensional mover's problem we consider the product 
space 

c = sr 2 x n. 

For a manipulator with m revolute joints, the C-Space may be approximated 
by n m , where 

n ra -nxnx...xn (to m). 

The three dimensional rotation group SO(3) can be approximated by a 
hemisphere of the 3-sphere (which is simply connected), or by IT 3 . II m is 
homeomorphic to the interior of the m-cube. This new product space C is simply 
a restricted configuration space where the piano is not allowed to spin around 
wildly. The approximation of .50(3) by a hemisphere of S'\ incidentally, is closely 
related to the employment of unit quaternions to represent rotations. The space of 
unit quaternions is precisely 5' J ; the two quaternions q and —q construct the same 
rotation, although they represent antipodal points on S'K When all antipodal points 
q and — q are identified, the projective 3-sphere, P 3 is obtained. P 3 is isomorphic 
to 50(3). It is of interest that Euler angle space, Q xi (see chapter 2) is essentially 
an approximation of 50(3) by 3? 3 (mod 27r), which is isomorphic to the 3-torus, 



231 



mavt mtmmM F* **- 1 -** -** "™ ''M 'M ^wi i pp ;«^ ^^^^ ^ 



when equivalent rotations are identified, If equivalent rotations are not identified, 
then Q 3 is isomorphic to S 8 . While the approximation of 80{3) by R 3 yields a 
simply-connected configuration space, from the point of view of an automated 
planner it has the undesirable effect of introducing an infinite number of goals in 
the rotational dimensions of configuration space, for every single goal in the space 
of Euclidean motions. For this reason the approximation provided by the 3- torus 
may be considered preferable. 
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Appendix III 

Integrating Local and Global Algorithms for the Find-Path Problem 

In Donald (1983a), we have discussed the integration of a global channel 

algorithm with a local C'-space algorithm to form a planning system for the 

find-path problem in D? 2 X S l . How can a three dimensional global, or channel 

algorithm be coupled with the planning system in 9? 3 X SO(S) described in previous 

chapters? More generally, what are the fundamental issues in integrating local and 

global geometric planning algorithms? In particular, 

(i) How can a global algorithm suggest paths, or equivalence classes of paths to a 
local algorithm? 

(ii) How can the relevant geometric constraints be identified and exploited by 
the local algorithm? Conversely, how can irrelevant geometric constraints be 
effectively ignored? 

(iii) How can global topological constraints, such as those arising from analysis 
of homotopy classes and fundamental groups, be propagated onto the (local) 
geometric structure examined by the local algorithm? 

In general, the design of a global algorithm will depend on the geometric 
constraints exploited by the companion local algorithm with which it will be 
coupled. Hence when we consider extending the channel algorithm of Donald 
(1983a) to the three dimensional find-path domain, we must specify what "target," 
local algorithm to use. A natural candidate is the local algorithm for find-path in 
D? 3 X SO(3), which we describe in chapters 1-2. 
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Path Suggestion. A problem which must be solved in any local/global find- 
path integration is how a global path may be suggested to a local algorithm. In 
two dimensions this was accomplished by segmenting the find-path problem into 
a sequence of sub-problems. The Suggestor strategy (chapter 2) is designed with 
this in mind. The verified points along the suggested path become planning islands 
in configuration space. The job of the local algorithm is then to connect up the 
planning islands and find a continuous path to the goal. 

Choosing Subgoals in Rotation Space. In a three-dimensional rotation space, 
the problem of selecting good rotational subgoals becomes more difficult. Much 
of the path-planning literature has been guilty of overlooking this difficulty. Such 
subgoals can be used in path-suggestion as we have described above. Even when 
the companion local algorithm is complete, a strategy for choosing good rotational 
subgoals is desirable, since it would allow the algorithm to converge faster. 

We have derived experimental strategies which consider alignments of the robot 
polyhedra with the faces and edges of obstacles. Every polyhedron's boundary 
contains alignable generators (faces and edges) which have an orientation, and 
non-alignable generators (vertices) which have no orientation. Two generators are 
said to be aligned when they are parallel, and the rotations in which they are 
aligned form connected alignment regions in 50(3). For example, two cubes are 
aligned when two faces are parallel, or an edge and a face are parallel. The channel 
construction is useful for identifying the obstacle surfaces which bound the proposed 
channel (in real space). Call this set of faces ft. Let ft denote the faces of the 
robot. The alignment regions can be considered for the generator pairs 

G Align = (ft X ft) \J{?r X 3ft) \}{df R X ft). 

We are only interested in applicable alignments, that is, an alignment of two 
(or more) generators where the generators can be brought into contact through 
some translational motion (this is our definition of applicability: see chapter 
3). Applicability may be determined by examining the applicability constraint 
functions (ACFs) introduced in chapter 3. Furthermore, in chapter 5, we showed 
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that alignments of edges and faces occurred exactly at the boundary of the 
applicability regions for C-functions. 

Every C-function is a partial function f{ on the configuration space 3ft' X SO(3), 
whose domain is 3ft 3 X Ai for A{ C 50(3). The set of alignment regions is obtained 
by the union of the boundaries of these applicability regions 

"align == \J O&i 
i 

for every C-function /;. Every point in R a ii gn lies in the kernel of some ACF 
gij : SO(3) — > 3ft for a C-function fa. In chapter 4 we showed how to derive charts 
for these boundaries and their intersection manifolds. 

In most find-path problems, the alignment regions in R a u (jn are complete as 
subgoals in rotation space — i.e., no other rotational subgoals need be considered in 
order to find a solution path (i f one exists). This makes a certair« intuitive sense: 
one might try aligning a large box with a narrow door-frame in order to squeeze 
it through. 1 However, in general there exist pathological cases in which this is not 
true (imagine a robot which looked like a polyhedral sea urchin), and the set of 
alignments is not complete as a set of subgoals. Furthermore, it is unsatisfying that 
the alignment analysis exploits strong constraints in the polyhedral domain of the 
classical Movers' problem, and does not appear to generalize well to linked-arm 
problems. 

We believe that it may be possible to overcome the problem of "star-shaped" robots 
by considering additional alignment regions obtained from faces and edges of the 
convex hull of such objects. Such an algorithm would have to deal similarly with 
"star-shaped" obstacles. Even these additional alignments may prove incomplete; 
however, they may have heuristic value. 

The problem of how a global algorithm can infer good rotational subgoals from 
the structure of real-space is one of the most interesting open problems in spatial 
planning. We conjecture that an answer may lie in the structure of the boundaries 



'We are not making any claims about human spatial reasoning here. 
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Figure II.l. Pathological example showing a robot A whose alignment regions do not include a 
rotation which helps get through the tight passage. 

of the applicability regions in 50(3). The algorithms we provide in chapter 5 for 

obtaining the applicability set decomposition may prove useful in computing this 

structure. Such a planner might slide along the intersections of ACF boundaries 

or level ACFs in rotation space, 2 much as our planner slides on the intersections 

of level C-surfaces in configuration space. The advantage of such an approach lies 

in reducing the (infinite) search for rotational subgoals to a finite combinatorial 

search along the ACF boundaries. 

Topological Constraints. Ideally, the global strategy should enforce the 

path-class criterion 3 for each sub-problem: no straight-line approximation for a 

subproblem may cross more than one equivalence class of paths. We begin by defining 

what a straight-line approximation means in Sfc 3 X 50(3). This requires some way of 

forming "linear combinations" of rotations. The definition of "linear combinations" 

2 Level ACFs arc defined in section 3.11. 
3 Sec and appendix II. 
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in 50(3) relates to a definition of convexity for 9? 3 X 50(3). In particular, we wish 
to determine whether C-space obstacles are convex. For if C-space obstacles are 
convex, then we could trivially bound the number of intersections any straight-line 
trajectory can make with any one obstacle. We present a conjecture that the 
C-space obstacles are non-convex. Finally, we discuss basic topological notions for 
formalizing our analysis of equivalence classes of paths. 

We require some way of forming "linear combinations" of rotations. The 
requisite algebraic structure is much like a module (over the reals), except that 
the group operation cannot be commutative. 4 . The group operation is composition 
of rotations. Let R(n, 0) denote rotation about the three dimensional vector h by 
angle 0. Scalar multiplication by a £ D? may be defined by 

aJZ(n,0) = Z(n,aO). 

By substituting the (non- commutative) composition of rotations for the group 
operation +, we obtain a natural definition for linear combinations, 

aR{n, 0) + (l- a)R{h', 0') = R{n, aO)R{n' , (1 - a)0') (JJ/.l) 

for < a < 1. R., of course, may be conveniently expressed by a unit quaternion. 

Suppose O/ 3 is a three-dimensional parameter space for 50(3) — that is, the 
domain of a chart for rotation space. For example, Q 3 might be the space of Euler 
angles (see chapter 2). It is possible to define linear combinations in the parameter 
space Sff 3 X 0/*. This seems unsatisfactory, since it makes the definition of linear 
combination — and more disturbingly, of convexity — dependent upon the chosen 
parameterization for 50(3). Observe that definition (III.l) for linear combinations 
is invariant for all parameterizations. 

Open Question: Under a definition of convexity invariant for all parameteriza- 
tions, show whether or not the C-space obstacles in 3J 3 X 50(3) (and 3J X 5 ) 



"•Recall that a module is defined as follows: If R is a commutative ring with identity, then M 
is a module over R if [M,+) is a commutative group, and scalar multiplication (r, M) \~> rM of 
elements M in M by r in R is associative and distributive (over +), and if \nM — M 
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are convex. Conjecture: We conjecture that C-space obstacles are non-convex. 
When 8ft 2 X 5 1 is approximated by 8ft 2 X [— tt, it) and embedded in 3ft 3 , the cor- 
responding C-space obstacles are non-convex using Euclidean linear combina- 
tions. Furthermore, in both 3ft 2 X 5 1 and 3ft 3 X SO(3), each obstacle manifold 
is the intersection of a finite number of half-hyperspaces of 3ft' X 50(3). 
Each half-hyperspace is in turn defined via a real-valued partial function 
/ : 3ft 3 X 50(3) — > 8ft. Using partial functions, arbitrary non-convex manifolds 
can be constructed. Showing that the obstacle manifolds could be represented 
by means of smooth, total functions would suggest convexity. Our analysis 
suggests that these functions must be partial, which in turn leads to the 
conjecture that the obstacle manifolds are non-convex. I 

The homotopy relation (see appendix II) partitions the function space of paths 
into equivalence classes. The image of one such equivalence class [/] is the region 
in C-space covered by the union of all the path images in [/]. The equivalence 
classes are determined both by the structure of the underlying Cspace , and 
by the C-space obstacles. Intuitively, the fundamental group 5 in a space Y is a 
topological invariant corresponding to the set of equivalence classes of paths in 
Y. The group operation corresponds to path composition ("pasting"), and for two 
paths /, g : I 1 -* Y where /(l) = g(Q), 

f/(2i), forte [<),§] 

f * g(t) = { 

\g(2t-l), forte[M- 

We think of / * g as the path whose first half is / and whose second half is g. 
The pasting operation * is well defined on path homotopy classes: 

[f]*[g] = [f*g], 

and exhibits groupoid properties. When an obstacle makes a hole in free-space, it 
augments the fundamental group for the space by adding an infinite (cyclic) number 



5 Soe appendix II for a review of elementary homotopy theory and a formal definition of the 
fundamental group. 
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Figure II.2. Pasting together of paths / and g. Some paths in the hotnotopic equivalence classes 
[/] and [</] are also shown. 

of equivalence classes. (For example, by puncturing the plane at the origin O, we 
obtain the classes of paths (l) not looping around O, (2) looping around O once, 
. . . , (n) looping n — 1 times around O, ...). The topology of the underlying C-space 
may be predetermined (see appendix II), but each new find-path environment 
generates different path homotopy classes. We wish to infer the equivalence classes 
in the fundamental group by their generators, i.e., the C-space obstacles. Since the 
C-space obstacles can be constructed from the real-space obstacles, we are actually 
attempting to compute path classes in C-space from the structure of real-space. 

In general, if free-space is connected, the image in C-space of even a single class 
of paths can cover all of free-space. However, we can impose a stronger condition 
which subsumes the path-class criterion. Let p be an injection of I 1 into C-space 
, which will represent some approximation of a solution path for a subproblem S. 
We wish to know whether the image of p can be expressed as the union of two 
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Figure H.3. The plane with a puncture (or obstacle) at the origin, showing paths / and g from 
x to u. / and g are not homotopirnlly equiv alent, and hence in different equivalence classes. 

sets of points, those lying in the image of one equivalence class of paths, and those 
lying in C-space obstacles bounding that image. In chapter 4, we showed how to 
intersect trajectories with C-surfaces in 5ft 2 X S 1 and # 3 X 50(3). We can intersect 
ptf 1 ) with all C-surfaces, and determine the intersection points. These intersection 
points indicate where it penetrates C-space obstacles, and are determined from its 
zero-crossings from free-space to forbidden space. 

Claim: That the image of p contains either no region or one connected region 
lying within any C-space obstacles is a sufficient (but not a necessary) condition 
satisfying the path-class criterion for sub-problems. 

Constraint Relevance. Another issue concerns how a global algorithm can 
characterize the relevant constraints for a local algorithm, and if necessary, impose 
additional, artiBcial constraints on the problem so that irrelevant constraints in 
the initial domain will be ignored. Because of the difficulties in maximizing channel 
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breadth in three dimensional channel construction, the artificial faces introduced 
by the current implementation may prove too restrictive, especially if the robot is 
large or non-convex. However, the channel construction is useful for identifying the 
obstacle surfaces which bound the proposed cl</ 'nel (in real space). Call this set of 
faces ?£. The C-surfaces generated by these faces may be exploited by the sliding 
and intersection experts (see chapter 2). Let Tr denote the faces of the robot. These 
preferred C-surfaces are identified with their generator pairs, namely 

Slide = (? R X vert{7 K )}\j(d7 R X d7 K }\j(vert{7u) X 7k). 

The identification of good C-surfaces to slide along addresses a central issue in 
local/global integration. At present, the sliding and intersection experts exploit only 
local geometric structure and planning history. The channel algorithm introduces 
a global criterion for selecting which C-surfaces to slide along, and for constructing 
intersection manifolds. The set of C-surfaces Slide specifies an implicit volume in 
configuration space which is closely related to the channel volume (in C-space ). 
This volume is obtained by extending the hyperplanes containing the C-surfaces 
past the obstacle boundaries until they intersect. Furthermore, Slide lies on the 
boundary of the image of the hypothesized equivalence class of paths. By choosing 
these interior surfaces as candidates for the sliding and intersection experts, global 
advice on constraint relevance is provided to the local C-space algorithm by the 
global channel algorithm. 
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Appendix IV 
A Listing of Macsyma Code 

In this appendix, we provide a listing of the Macsyma code to produce optimized 
Lisp procedures for computing the coefficient functions of the canonical linear 
forms and trigonometric quadratic forms of the type (a), (b), and (c) C-functions, 
and type (a) and (b) ACF clauses for 5ft 3 X SO(3). Using these forms, the 
intersection manifolds, type (c) ACFs, and disambiguating applicability constraints 
are constructed in the manner described in the text. 

We also list (in Macsyma) the resulting combined forms for the C-functions and 
ACFs. Note that the type (c) C-function is "over a page long." 

Rotations are specified by means of the Macsyma functions RotateV ector(x : 
vector) and Trans form(x : plane or vector). Rotations are implemented using 
the Euler angles. However, by changing these two functions, any representation 
for rotations-such as quaternions, spherical angles, or wrist angles for a cartesian 
manipulator-may be employed. This corresponds to reparameterizing 50(3), and 
results in different charts for the level C-surfaces, intersection manifolds, and ACF 
manifolds. 
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/• Display and Grind function. If OPTIMIZE.FLAG is TRUE then 
we actually store the OPTIMIZED expression •/ 

print("Setting Grind to TRUE...")S 

Grind:True$ 

0ptimi2e_Flag: TRUES 

/• OPIMPREFIX: X •/ 

Display_and_Grind(exp):» 

Blocti{[label]. 

if OPT1MIZE.FLAG THEN 
(print(EXP). 

Print(" Optimizing. .. ). 
Exp: Optimize(Exp)). 
label:Ldisp(exp), 
if Grind then 

(print(" Ground, becomes: 

-, label[l]." : "). 

grind(exp)). 

print(" ")• 
label[l])$ 

/• Utility function. Is the expression EXP free of all the VARS (a list?) •/ 

Free_Of_Vars(Vars, Exp) :- 
block([freedon], 

freedom:true. 
(for var 1n Vars unless freedom • false do 

(if Not FreeOf(Var, Exp) then freedom: false)), 
if Not freedom then 
Print("[Exp contains Major Variables. Recursively Analyze...] ). 
freedom)$ . 

/• here we define Canonical Linear Form to be simply the 
expression of the constraint as a linear function 
in X. Y. and Z •/ 

Canonical_linear_variables: [X, Y. Z]$ 

Canonical_linear_form(Exp) :- 

IsolateN(Exp. Canonical_Tinear_variab1es)$ 
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/. Bruce Donald (BRD60Z) analyze hairy expressions -•- MACSYMA -•- 

,. 'Analyze linear Forms : given the "chief vars" in RATVARS. 
negate intermediate labels for all the coef f icients 
If these vars and return the "simplified" bilinear form. 
Slcur'slvel Cam ANALVZC.BILINEAR.fORM S« > thai .the in ermedi.te 
labels are truly "constants" relative to the Ratvars / 

/• typically. ratvars:[x.y.z.psi.theta,phi] •/ 

/. IsolateN works like ISOLATE but for N variables in a list, 
on a bilinear form •/ 

lsolateN(Exp. Hvars) :- 
Block([Save_Ratvars. Iform], 

VL Save.ratVars: ratvars. 

RatVars: Nvars. 

Iform: Analyre_Bil inear_form(Exp) , 

Ratvars: Save_Ra*v al- 
iform)* 

simple_Analyze_depth: 4$ 

Analyze_Bilinear_Form(exp) :- Labe 

block([power. Coef, Rat_txp. Leu. n>»=. 
print{"Analyzing:"). 
ldisp( reveal (Exp, simp! e_analyze_depth}). 

Left: rat(exp). Sum: 0, 
for var in Ratvars do 
(Power: Hipow(Left. Var). 
if power > 2 then 

(1 ° Se: Er^Tarning: Not a Bilinear Form because of ". lose)). 
Coef: ratcoeff(left. var. 2). 
if not (Coef - 0) then 

( Print( "The coefficient of ". var 2. is ). 
Label :Display_and_grind(Coef). 
if not Free_of_vars(ratvars.Coef) 

then Label: Analyze_Bilinear_form(CoeT). 
Sum: Sum + label • Var-2, 
Left: rat(left - Coef • (Var *2)))), 
print("Mixed terms: "). 
for var in Ratvars do 
(for var2 in Ratvars do 
if var #var2 then 
(Coef: ratcoeff(Left. var»var2. 1). 
•if not (Coef • 0) then 

( Print( "The coefficient of ". Var«var2." Is "). 
label :Display_and_grind(Coef). 
if Not Free_Of_vars(RatVars. Coef) 

then Label: Analyze_Bilinear_form(Coef ). 
Sum: Sum ♦ label • var* var2. 
Left: rat(left - Coef • (Var • var2))))). 
pr1nt(" Linear Terms: "). 
for var 1n Ratvars do 

(coef: ratcoeff(left. var, 1). 

if not (coef • 0) then m 

( print("The Coefficient of .var. it ). 
label :Display_and_grind(Coef). 
if Not Free_Of_vars(RatVars. Coef) 

then Label: Analyze_Bilinear_form(CoeT). 
Sum: Sum + label»var. 
Left: rat(left - Coef* Var))). 

1f Left # then 

(Print(" And the constant term is ). 
Label: Display_and_grind(left). 
sum: Sum + Label), 
Print(" Yielding :"). Oisplay_and_grind(Sum) . Sum)J 
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/• Bruce R. Donald. (BRD802) -•- Mode:Macsyma -•- 
Attempt to express applicability constraints for 
C-surfaces in R*3 \cross S*3 •/ 

/• PRODUCTION VERSION — i.e., for production of LISP code •/ 
BTHETA: [phi, theta. psi]S 

if .Euler_Rotation_equations_loaded • TRUE then "OK" 
else Batchload ([rotate. mac]); 

shorten(exp):- subst( S.sin. subst(C. cos, exp))$ 

ratvars:[ sin(phi).cos(phi).sin(theta).cos(theta). sin(psi).cos(ps1)]; 

/• Each constraint is of the form •/ 

/• vectors: "/ 

u(i) :• [ux[i],uy[i],uz[i]]; 

v(i) :■ [vx[i].vy[i].vz[1]]; 

/• Normal for plane eq •/ 

n(i) :■ [nx[i].ny[i].nz[i].nd[i]]; 

/• Here we define functions to generate the applicability constraints, 
the arguments are: Bn : a vertex in 3-space, which we use to 

measure distance to the plane. 
v : a vertex which we insist must be ON the plane. 
N : a plane (4-vector) 

c : the "height" of the level surface in S*3. 
if 0, corresponds to the maximim boundary 
of the applicability clause (eg edge-face 
contact). 
•/ 

R3_projection(Vec) :- [vec[l], vec[2], vec[3]]$ 

/ 
Type_B_Clause(N, bn. v. c) :- 

simp_3( ,_ .. 

(R3 Projection(N) . Rotate_vector(Bn)) 

- (R3~Projection(N) . Rotate_vector(v)) 

- c); 

Type_A_clause(N. bn, v. c) :• 

Block ([N.THETA], 
ll_THETA: part(transf orm(Euler_Inverse, «;. ij. 

simp_3( 

(R3_Projection(N_THETA) . bn) 

- (R3_Projection(N_theta) . v) 

-c)); 

Midp(a.b) :■ a + (b-a)/Z$ 

Type_Cl_Clause(nl. n2. al. a2, cl. cZ) :• 
timp_3( 

- type_b_clause(nl. al. midp(al.a2), cl) 

• typelb_clause(n2. al. midp(al.a2). c2)); 

Type_C2_Clause(Nl.N2. bl. b2. cl. c2) :- 
simp_3( 
- type_a_clause(nl, bl, midp(bl,b2). cl) 
• type_a_clause(n2. b2. midp(bl,b2). c2)); 

/• lev[i] 1s just a CONSTANT to construct a Level surface on S*3 which 
is applicable */ 
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/• KC. VC, ZC. WC are Coordinate" accessor functions (MACROS) in LISP •/ 

tvoe B_Aclause[l]: 

iype B Clause([xc(N), YC(K). ZC(N). WC(N)]. 
[xc(u). yc(u). zc(u)]. 
[xc(v), yc(v). zc(v)]. 
Level); 

/• bm : type b_clause( n(il). v(jl). u(U). lev[l]) 
B[2] : type_b_dause( n(i2), v(j2). u(*2). lev[2]) •/ 

Type_A_Ac1ause[l]: 
type_A_Clause([xc(N). YC(N). ZC(N). WC(N)]. 
[xc(u). yc(u). zc(u)]. 
[xc(v). yc(v). zc(v)]. 
Level): 

/• am : type.a clause( n(ll). u(ml). v(pl). lev[3]) 
a[2] : type_a_clause( n(12). u(mZ), «{p2). lev[4]) •/ 

/• C 1[1] : type_ct.clause( n(bl). n(b2). v(ail). v(ai2). lev[5], lev[6]) 
Cl2[l] : type_c2_clause( n(al). n(a2). u(bjl). u(bj2). lev[7], lev[8]) •/ 

/• Grind the results... •/ 

Type_A_Adause[l3 :" )$ Grind{Type_A_Aclause[l])S 

print(" . -.,»• 

Type_B_Aclause[l] : ")S Grind(type_B_Adause[l])S 
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/• bruce r. donald. cspace constraints in 3-dimensions. -•- macsyma -•- •/ 

/• production-version: i.e. produce lisp code... 

xc. yc and zc are baccessor macros for components of vectors •/ 

/• this next section contains the equations for c-surfaces In 
r*3 x s*3. see aim 606, tomas' spatial planning paper for details. •/ 

/• load euler rotation equations •/ 
if euler_rotation_equations_loaded ■ true 

then "ok" 

else (batchload("usrd$:[brd.prod]rotate.mac"), 
euler_rotation_equations_loaded:true); 

/* a vector in r*3: •/ 

xvec : [x, y. «]•' 

ratvars: [x. y, z, 

sin(phi). cos(phi), sin( theta), cos(theta). 

sin(psi). cos(psi) ]; 

/• a vertex on a , and a(i+l) •/ 

ai : [xc(ai) . yc(ai). zc(ai)]; 

aione: [xc(aione). yc(aione). zc(aione)]; 

/• a vertex on b . and b(j+l) •/ 

bj: [xc(bj). yc(bj). zc(bj)]; 

bjone: [xc(bjone). yc(bjone), zc(bjone)]; 

/• the normal to a face fi on a •/ 

nfi: [ xc(nfi). yc(nfi). zc(nfi). wc(nfi)]; 

/• the normal to a face gj on b •/ 

ngj : [xc(ngj). yc(ngj), zc(ngj). »c(ngj)]; 

/• type a surface, rotate the normal: •/ 

R3_projection(Vec) :- [vec[l]. vec[2], vec[3]]$ 

Rot_Nf1: part(Transform(Euler_Inverse,nfi).l); 

N_a: R3_Projection(Rot_Nfi); 

Inner_Product_Term: (Rotate_vector(ai) ♦ bj); 

A_5: M_a . Xvec - (N_a . Inner_Product_Term); 

/• Type B Surface •/ 

K_b: R3_projection(NgJ); 

B_6 : N_b . Xvec - (N_b . Inner_Product_Term); 

/• Type C surface (I) •/ 

Edge.a: rotate_vector{aione) - rotate_vector(ai); 

Edge_b: Bjone - bj; 

W_C: Cross( Edge_a, Edge.b); 

C_5: N_C . Xvec - (N_C . Inner_Product_Term) ; 
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/• -•- MODE: MACSYMA -•- 

(HERE ARE THE DEFINITIONS Of TYPE (A) (B) AND (C) C-SURFACES FOR 
THE 6D0F MOVERS PROBLEM. AND TYPE (A) AND (B) APPLICABILITY 
CONSTRAINTS . Output of CSPACE and APPLIC under PRODUCE for 
production run. ) •/ 

Type_A_Csurface[l] : 

(("(XC(NFI)»X-XC(BJ)'XC(NFI))*COS(PHIh(XC(NFI)'Y-YC(BJ)»XC(NFI))»SIN(PHI)) 

•COS(THETA) 
+ (-XC(NFI)»Z+ZC(BJ)*XC(NFI))'SIN(THETA)+(YC(NFI)»Y-YC(BJ)»YC(NFI))*COS(PHl) 

+(-YC(NFI)»X»XC(BJ)»YC(NFI))*SIN(PHl)-YC(NFI)) 

•COS(PSI) 

+(((-YC(NFl)»X*XC(BJ)»YC(NFI))»COS(PHI) 
+(-YC(NFI)»Y+YC(BJ)»YC(NFI))*SIN(PHI)) 

•COS(THETA) 
♦(yC(NFI)*2-2C(BJ)*YC(Nn))»SIN(THETA)-(XC(NFI)»Y-YC(BJ)'XC(NFI))*COS(PHI) 

4(-XC(NFI)»X+XC(BJ)»XC(NFI))»SIN(PHI)-XC(HFI)) 
•SIN(PSI)*(ZC(NFI)*Z-ZC(BJ)VC(NFI))»COS(THETA) 
-►((ZC<NFI)»X-XC(BJ)«ZC(NFI))»C0S(PHI) + (ZC(NFI)»Y-YC(BJ)»ZC(NFI))»S1N(PHI)) 
*S1N(THETA)-ZC{AI)"ZC(NFI)-YC(AI)»YC(NFI)-XC(AI)»XC(NFI)S 



Type_B_Csurface[l] : 

(<-XC(AI)*XC(NGJ)'COS(PHI)-XC(AI)»YC(NGJ)»SIN(PHI))»COS(THETA) 
+XC(AI)»ZC(NGJ)'SIN{THETA)-YC{AI)«YC(NGJ)*COS(PHI)+YC(AI)»XC(NGJ)»SIN(PHI)) 

•COS(PSI) 

+{(YC(AI)»XC(NGJ)»COS(PHn+YC(AI)»YC(NGJ)*SIN{PHI))»COS(THETA) 
-YC(AI)»ZC(NGJ)»SIN(THETA)-XC(A1)'YC(NGJ)*C0S(PHI)+XC(AI)»XC(NGJ)»SIN(PHI)) 

•SIN<PSI)-ZC(AI)»ZC(NGJ)»COS(THETA) 
+{-ZC(AI)»XC(NGJ)'COS(PHI)-ZC(AI)«YC(NGJ)»SIN(PHI))»SIN(THETA) 
-YC(NGJ)»COS(PHI)+XC(NGJ)»SIN(PHI)-ZC(NGJ)*Z*YC(NGJ)*Y+XC(NGJ)»X 
-ZC(BJ)»ZC(NGJ)-YC(BJ)»YC(NGJ)-XC(BJ)*XC(NGJ)$ 

Type_C_Csurface[l] : 

(((((XC(AIONE)-XC(AI))»YC(BJONE)+(-XC(AIONE)+XC(AI))»YC(BJ))»Z 
+((-XC(AIONE)+XC(AI))«ZC(BJONE)+(XC(AIONE)-XC(AI))»ZC(BJ))«Y 
*(XC(AIONE)-XC(AI))»YC(BJ)»ZC(BJONE)+(-XC(A10NE)+XC(AI))»ZC(BJ)*YC(BJONE) 
+(-YC(AI)»ZC(AIONE)+ZC(AI)»YC(AIONE))*XC(BJONE) 
+(YC(AI)»ZC(AIONE)-ZC(AI)»YC(AIONE))*XC(BJ)) 

•COS(PHI) 
♦(((-XC(AI0NE)+XC(AI))»XC(BJ0NE)+(XC(AIONE)-XC(AI))»XC(BJ))»Z 

+((XC(AIONE)-XC(AI))«ZC(BJONE)*(-XC(AIONE)+XC(AI))»ZC(BJ))«X 

+(-XC(AIONE)+XC(AI))*XC(BJ)»ZC(BJONE) 

+(-YC(AI)«ZC(A10NE)+ZC(AI)»YC(AIONE))«YC(BJONE) 

+(XC(AIONE)-XC(AI))»ZC(BJ)»XC(BJONE) 

+(YC(AI)»ZC(AIONE)-ZC(AI)»YC(A10NE))*YC(BJ)) 

•SIN(PHI)+(XC(AIONE)-XC(AI))»ZC(BJONE)+(-XC(AIONE)+XC(M))»ZC(BJ)) 

•COS(THETA) 
((<XC(AIONE)-XC(AI))*XC(BJONE)+(-XC(AIONE)+XC(AI))»XC(BJ))»COS(PHI) 

+((XC(AIONE)-XC(AI))»YC{BJONE)+(-XC(AIONE)+XC(AI))»YC(BJ))»SIN(PHI) 

+((-XC(AIONE)+XC(AI))»XC(eJONE)+(XC(AIONE)-XC(AI))»XC(BJ))»Y 

+((XC(AIONE)-XC(AI))»YC(BJONE)+(-XC(A10NE)+XC(AI))»YC{BJ))»X 

+(YC(AI)»ZC(AIONE)-ZC(AI)»YC(AIONE))»ZC(BJONE) 

+(-XC(AIONE)+XC(AI))'XC(BJ)»YC(BJONE)+(XC(AIONE)-XC(AI))»YC(BJ)«XC(BJONE) 

♦(-YC(AI)»ZC(AIONE)+ZC{Al)»YC(AIONE))»ZC(BJ)) 

•SIN(THETA) 
+(((-YC(AIONE)+YC(AI))»XC(BJONE)+(YC(AIONE)-YC(AI))»XC(BJ))»Z 
+((YC(AIONE)-YC(AI))'ZC(BJONE)+(-YC(AIONE)+YC(Al))»ZC(BJ))»X 
♦(-YC(AIONE)+YC(AI))»XC(BJ)»ZC{BJONE) 
+(XC(AI)»ZC(AIONE)-ZC(AI)'XC(A10NE)) , YC(BJONE) 
■f(YC(AIONE)-YC(AI))'ZC(BJ)»XC(BJONE) 
+(-XC{AI)»ZC(AIONE)+ZC(AI)»XC(AIONE))«YC(BJ)) 



♦ 
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! -YC !!Se *?C II .VC(BJ)*ZC(BJ0llb*(W(M(»E)-YC(»I))-2C(BJJ-VC(BJ0iE) 

♦ -XC AI)'ZC(AIONE)+ZC(AI)»XC(AIONE))»XC(BJONE) 
+ (XC(AI)*ZC(AIONE)-ZC(AI)»XC(AIONE))»XC(BJ)) 

•SIN(PHI)) 

Ir?m-YC(AIONEVYC(AI))*YC(BJONE)*(YC(AIONE)-YC(AI))*YC(BJ))«Z 
1 YCfAlONH-YaAl )»ZC(BJONE)^-YC{A10NE) + YC(Al))'ZC(BJ))«Y r , oinMr . 

I -S «S /yC(AI))'YC(BJ)-ZC(BJOn[).(YC(AIONE)-YC(AI))-ZC(BJ)-YC(BJOIIE) 

♦ (-XC(AIWC(A10NE)-ZC(Al)»XC(AIONE))»XC(BJONE) 
+ (XC(AI)»ZC(A10NE)-ZC(AI)'XC(AIONE))*XC(BJ)) 

!r?fYUAIONE1-YC(AI))>XC(BJONE)*(-YC(AIONE) + YC(AI))*XC(BJ))«Z 
I -YnAIONE)lYC(Al))*ZC(BJONE) + (YC(AIO N E)-YC(AI)).ZC(BJ))«X 

+ (YC(AIONE)-YC(AI))'XC(BJ)'ZC(BJONE) 

♦ -XC(AI)»ZC(AIONE)+ZC{AI)'XC(AIONE))*YC(BJONE) 
+ (-YC(AIONE)+YC<AI))*ZC(BJ)»XC(BJONE) 

♦ XC(AI)»ZC(A10NE)-ZC(AI)»XC(AIONE))'YC(BJ)) „„„.„,„,, 
4lN(PHI)'(-YC(AIONE)iYC(AI))'ZC(BJONE) + (YC{AIONE)-YC(AI))»ZC(BJ)) 

■ S -YC IlONE +YC AI -YC BJOHE)* YC(AIONE)-YC(AI))-YC(BJ «SIII{PHI) 
t YC(AIONE -YC(AI) •XC(BJONE)i(-YC(AIONE) + YC(AI))«XC(BJ Y 
+ -YC(AIONn + YC(Al))«YC(BJONE) + (YC(AIONE)-YC(AI))»YC(BJ))«X 

+ (-XC(AI)'ZC(AIONE)+ZC(AI)'XC(A10NE))»ZC(BJ)) 

i XC(AIONE)-XC<AI))»ZC(BJONE) + (-XC(AIONE)+XC(AI))*ZC(BJ))»X 

♦(-XC(AIONE)+XC(AI))»XC(BJ)'ZC(BOONE) 

+(-YC AI)»ZC(AIONE)+ZC(AI)»YC(AIONE))»YC(BJONE) 

+ (XC(AIONE)-XC(AI))»ZC(BJ)»XC(BJONE) 

+ {YC(AI)«ZC(AIONE)-ZC(AI)»YC(AION£))»YC{BJ)) 

i XC(AIONE)-XC(AI))»ZC(BJONE)^-XC(AIONE *XC(A ))'ZC BJ))*Y 

♦ -XC(AIONE)+XC(AI))*YC(BJ)*ZC(BJONE)+(XC(AIONE)-XC(AI))«ZC(BJ) YC(BJONt; 

+ YC(AlWC(AIONE)-ZC(AI)«YC(AIONE))'XC(BJONE) 

+ (-YC(AI)'ZC(AIONE)+ZC(AI)»YC(AIONE))»XC(BJ)) 

•SIH(PHI)) 
+ ffJ-?r?MONEHZCfAn^XC{BJONE)+(ZC(AIONE)-ZC(AI))'XC(BJ))»COS(PHI) 
i -ZC I IZ t C M -YC \lY0Ht ♦ ZC MOBE -ZC(*I))-VC(BJ))«SXB(PHI) 

: 5SJS s;iii;i;s^^ c c!ii^!srAS!!:^K!r.; 

+(XC(AI)»YC(AIONE)-YC(AI)'XC(A10NE))*ZC(BJ)) 

wmZC(A10NE)-ZC(AI))nC(BJONE) + (-ZC(AIONE)*ZC(AI))«YC(BJ))«Z 

i - IloNE *ZC AI -ZC ■«■£ ♦ 2C(AIOME)-2«M)).ZC(W))-Y 

♦ ZC(A10NE)-ZC(AI))*YC(BJ)«ZC(BJONE)+(-ZC(AIONE)*ZC(AI))'ZC(BJ)»YC(BJONE) 

+(-XC(AI)'YC(AIONE)+YC(AI)»XC(AIONE))'XC(BJONE) 
♦(XC(AI)'YC(AIONE)-YC(AI)'XC(AIONE))»XC(BJ)) 

I(((-K(MOME)*ZC(»I))'XC(BJO«)*(ZC(AIOHE)-2C(M))*XC(BJ» 
I ZC(AIONE)-ZC(AI))'ZC(BJONE) + (-ZC(AIONEhZC(AI))'ZC(BJ))«X 

+(-ZC(AIONE)+ZC<AI))'XC(BJ)*2C(BJONE) /o , amcv 
♦ -XC Al)'YC(AIONE)*YC(AI)*XC(AIONE))»YC(BJOIIE> 
♦(ZC(AIONE)-ZC(AI))'ZC(BJ)'XC(BJONE) 
+(XC(An»YC(AIONE)-YC(Al)»XC(AIONE))*YC(BJ)) 
•SIN(PHI)I(ZC(AIOliE)-ZC(AI))«ZC(BJONE) + (-2C(AIONE)*ZC(AI))»ZC(BJ)) 

•SIN(THETA)$ 
/• Herer are tne Applicability constraints •/ 
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Type_A_Aclause[l] : 

(f(XC(U)-XC(V))'XC(N)'COS(PHI) + (YC(U)-YC(V))»XC(N)»SIN(PHI))-COS(THEU) 

i(-ZC(U) + ZC(V))*XC(N)»SIN(THETA) + (YC(0)-YC(V))«YC(N)»COS(PHl) 

♦(-XC(U)+XC(V))»YC(N)»SIN(PHI)) 

•COS(PSI) 
+{((-XC(U)+XC(V))»YC(N)»COS(PHI) + (-YC(U)*YC(V))»YC(N)*SIN(PHI))»COS(THETA) 

+<ZC(U)-ZC(V))'YC(N)'SIN(THETA) + (YC(U)-YC(V))»XC(N)'COS(PHI) 
♦<-XC(U)+XC(V))»XC(N)»SIN(PHI)) 

•SIN(PSI)+(ZC(U)-ZC(V))»ZC(N)»COS(THETA) , rwrl « 

+ ((XC U)-XC V) .ZC(N)»COS(PHI)*(YC(U)-YC(V))»ZC(N)»SIN(PHI))»SIN(THETA)-LEVEL$ 



Type_B_Aclause[l3 : 

(((-XC(N)»XC(V) + XC(N)*XC(U))«COS(PHI) + (-YC(N)'XC(V)+YC(N)«XC(U))'SIN(PHI)) 

!(ZC(N"*XC(V)-ZC(N)'XC(U))»SIN(THETA) + (-YC{N)nC(V)+YC(N)»YC(U))*COS(PHI) 

+(XC(N)»YC(V)-XC(N)»YC(U))*SIN(PHI)) 

•COS(PSI) 
+ (((XC(N)»YC(V)-XC(N)»YC(U))*COS(PHI) + (YC(N)»YC(V)-YC(N)»YC(U))'SIN(PHI)) 

I(-ZC(N)«YC(V) + ZC(N)»YC{U))»SlN(THETA)+(-YC(N)»XC(V)+YC(N)»XC(U))»COS{PHI) 

+(XC(N)»XC(V)-XC(N)»XC(U))*S1N(PHI)) 
•SIN(PSIW-ZC(N)»ZC(V)+ZC(N)'ZC(U))*COS(THETA) 
+((-XC(N)*ZC(V)*XC(N)»ZC(U))»COS(PHI)+(-YC(N)»ZC(V)+YC(N)»ZC(U))»SIN(PHI)) 

•SIN(THETA)-LEVEL$ 
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/• -•- mode: macsyma -•- 
(Bruce Donald. Here we Express a Constraint (C-surface or 
Acf) in Canonical Linear Form and Canonical Trig form. 

Given a CONSTRAINT which is either a 

C-surface or an ACF (applicability clause function) 

and a variable (VAR) we solve for the variable) •/ 

load_up():" /• load necessary files •/ 

block([], /• for solving •/ 

if ALL_DEFS_LOADED ■ TRUE then "OK" 

else batchload("usrd$:[brd.prod]defabc.nac"), 
batch load([intabc,nac]), 
batchload("usrd$:[brd.prod]analyze2.mac"), 
RatVars: [x, y, 2, 

sin(phi), cos(phi). sin( theta), cos(theta), 
sin(psi), cos(psi) ], 
Angles: [Phi, Psi. theta])$ 

Load_up()$ 

/* give us an "explicit" tangent space */ 

s(var) :• sin(var)S 
c(var) :• cos(var)$ 
Build_Manifold()$ /• Rebuild Manifold •/ 

solve_for_angle(exp.var):" /• Solve for COS(var) •/ 

block([Rats. Rl. R2, R3], 

rats: ratvars, 

Ratvars: [c(var), s(var)], 

print("Simplifying. . ."). 

Rl: IsolateN(exp. Ratvars). 

Print("Eliminating ". s(var).". . ."), 

R2: Elininate[l](Rl, Var). 

Ldisp(R2), 

Print("Solving for ", c(var),". . ."), 

R3: so1ve(R2,c(var)), 

display_and_grind(R3), 

ratvars:rats, 

r3)$ 

Solve_for_X(Exp, Xvar):« /• Solve for Any Var •/ 

Block([RatS. rl. R2], 

Rats: ratvars. 

Ratvars: [Xvar], 

print ("Simplifying. .."), 

Rl: IsolateN(Exp, Ratvars). 

print(" Solving for ", Xvar."..."), 

r2: solve(Rl, Xvar), 

Display_and_grind(r2). 

ratvars: rats, 

R2)$ 

Solve_test():" /• Test the Solution Routines •/ 

t>1ock([], 

grind(solve_test), 

grind.false, 

Solve_for_angl«(Csl, Ph1), 

Solve_for_X(CSl. X), 

/• SOlve a type (a) surface for PSI and Y •/ 

AS1: SinCos_to_CS(type_A_CSurface[l]). 

Solve_for_angle(ASl, Psi), 
Solve_for_X(ASl, y). 

/• SOlve a type (B) surface for PSI and Y •/ 
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BS1: SinCos_to_CS(type_B_CSurface[l]). 

Solve_for_angle(BSl. Psn). 
Solve_for_X(BSl. y). 

WotifyO 
)$ 

/• CExpress expresses things Canonically •/ 

CExpress(Exp. Type) :■ 

BLock([j. 

If Type * C_Surface then 
(Print("Canonical Linear Form ..."). 
Canon ical_l inear_f orm( Exp)). 
Print{"Solving for Angles..."), 
for Var in Angles do 

(So1ve_for_angle(Exp. Var)))S 

/• Here we Do the Expression. Now to get Ground forms, just change GRIND, etc. •/ 

DO_CExpress():« 

BL ° Ckl[3 Grind: False. /• Here it is.. V 

Mll(labels). „ , . 

CExpress(type_A_Csurface[l], 'C_Surface). 

CExpress(type_B_Csurface[l], 'C_Surface). 

CExpress(type_C_Csurface[l]. "C.Surface). 

CExpress (type_A_Aclause[l], *ACF). 

CExpress (type_B_AClause[l]. 'ACF))S 
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/• -•- Macsyma -•- BRD90Z (Bruce R. Donald). 

Sove intersections of C_surfaces and A_clauses. 
for the 6dof mowers problem. •/ 

Angles: [theta. Phi, Psi]$ 

/• allow some simplification into C and S terms •/ 

S1nCos_to_CS (exp) :■ 
B1ock([E], 

E: exp. 

for Var in Angles Do 

E: ratsubst{s[var], sin(var), 

ratsubst(c[var], cos(var). E)). 

rat(E))$ 

/• permit the inverse •/ 

CS_to_SinCos (exp) :• 
Block([E], 

E: exp, 

for Var in Angles Do 

E: ratsubst(sin(var), s[var], 

ratsubst(cos(war).c[var], E)), 

E)$ 

/• The Tangent Space Manifold •/ 

/• DO it yourself. pa,l I batchload([defabc,mac. "usrdS", brd]) •/ 

/• Short form functions •/ 

s(var) :■ $[var]$ 
c(var) :- c[var]$ 

/• Simplify 1f possible? use the s/ctvar] form though. •/ 

Simp_l(Exp. Var) :• ratsubst(l. s(var)*2 + c(var)*2, txp)$ 7 
simp_3(Exp) :• Rat{ 

Simp_l( Simp_l( Simp_l(Exp, Phi). Theta). Psi))$ 

manifo1d(var):« s(var)-2 ♦ c(var)*2 • IS 

Build_manifold() :• 
81ock([]. 

Man[theta]: manifold(theta). 

Wan[Phi] : Manifold(phl). 

Man[Psi] : Manifold(psi))S 

Build_manifold()$ 

/• Eliminate^] eliminates the dual trig term. SM 1s parameterized by 
one variable (var). Eliminates SIN(VAR) from EXP •/ 

•liminate[l](exp. var) :• 

b1ock([]. 

Tempi: rat(part( 

eliminate([exp. Man[var]]. [s(var)]).l)) 

)$ 

/• Eliminate^] calls eliminate^] twice, and eliminates the resultants. 
Hence the intersection of two level surfaces on S*3 is 
parameterized by a one param. family. Eliminates Sin/cos Varl/Var2 
from Exp. •/ 

US£_CS_FORM: True$ 

eliminate[2](expl, exp2, varl. var2) :• 
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block( [Rl. R2. R3. R4. R5. r6. /• Temp results •/ 

Lrl. Lr2, Lr3. Lr4. Lr5, lr6]. /• Their Labels •/ 
RatVars: [x. y, i, 

s(phi). c(phi), s( theta), c(theta), 
s(psi), c(psi) ]. 
if Use_CS_FORM then 

(expl: SinCos_TO_CS(expl), 
exp2: SinCos_To_CS(exp2)), 
printC'Eliminate ",s(varl)." from expression 1:"). 
rl: elininate[l]{expl, varl), 
Lrl:ldisp(rl). 

printC'Eliminate ",s(varl)," from expression 2:"). 
r2: eliroinate[l](exp2. varl), 
Lr2:ldisp(r2). 
printC'Eliminate ",c(varl)." from " ,append(LRl, LR2)), 
r3: rat(eliminate([rl.r2].[c(varl)3)). 
Lr3: Ldisp(R3). 
printC'Eliminate ",s(var2)," from ".Lr3). 
R4: eliminate[l](R3. var2), 
Lr4: Ldisp(R4). 
Print(" Solve ". U4." for ", c(var2)), 
R5: rat(Solve(R4. c(var2))). 
Lr5: Ldisp(R5). 
print("Finally. solve ".Lrl," for ",c(varl)). 
R6: rat(solve(Rl, c(varl))). 
Lr6: ldisp(R6). 
Append(lr2.1r2.1r3.1r4,1r5,1r6))S 

Test():- 

eliminate[2](type_A_Aclause[lj, 
type_A_Aclause[2], 
Phi. 
Ps1)S 
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/• -•- Mode: macsyma -•- 
(File to Run Production of 3d Space equations. 
Bruce R. Donald. MIT AI LAB ) •/ 

/• Define the type ABC constraints. •/ 

Define_ABC():» 
bl0Ck(rj. 

batch("USRDS:[brd.prod]Cspace.mac"))S 

/* Define the applicability Constraints •/ 

Define_Applic():» 
Block{[], 

Batch("USRD$:[brd.prod]Applic.mac"))$ 

/• Define Both •/ 

Produce_Defs():» 
block([], 

Writefile("usrd$: [brd.prod]Produce.1og"), 

Batch ("usrd$:[brd.prod]Rotate. mac"), 

Define_ABC(). 

Define_Applic{). 

ClosefileO, 

*otify())S 

Cold_Restart():- 
block([]. 

batchload("sys$login:utils.mac"), 

batchload("usrd$:[brd.prod]produce.mac"), 

Produce_defs{))S 

/• Here's a function to save labels for you. •/ 

SaveJ)abels(file):- 
block([]. 

?0pen_output_file(f1le), 

for Label in Append( rewerse(labels(e)). reverse(labels(d))) 

DO 

(?Grind_TO_FILE(Label). Prlnt(Lebel)). 
?Close_0utput_f1 !•())$ 

/• Produce the EXPRESS file, parsing into solutions and coefficients. •/ 

Produce_Express(): B 
block([], 

*ritefile("usrd$: [brd.prod3Express.log"), 

batch("usrd$:[brd.prod]Express.mac"), 

DO_CExpress(). 

Print(" Saving Labels in LSP file..."). 

Save Label s("USRDS:[brd.prod]EXPRESS. LSP"). 

Not1fy())$ 

/• this function produces EVERYTHING. •/ 

Produce_ALL{):« 
BL0CK([], 

Produce_Defs(). 

ALL_DEFS_LOADED: TRUE. 

Kill(Nfi. Ngj. AI. Alone. Bj. Bjone), 

Type_A_Csurface[l] : A_6. 

Type_B_Csurface[l]: B_5. 

Type_C_Csurface[l]: C_6. 

Produce_express(). 

closefi1e())$ 
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/• Bruce R. Donald (BRD80Z) Euler Rotations for Macsyma. Sea -•- MACSYMA -•■ 
Paul, p 45 •/ 

c(angle):* cos(anQle)$ 
s(angle):- sin(angle)$ 

Rot_z_psi: 

matrix( 

[ c(psi). -s(psi). 0. 0], 

[ s(psi). c(psi). 0. 0]. 

[ o; 0. 1. 0], 

[ O. 0. 0. 1]); 

Rot_y_theta: matrix( 
[ c(theta). 0. s(theta). 0], 
[ 0. 1. 0. 0], 
f-s(theta). 0. c(theta). 0], 
[ 0. 0. 0. 1]): 

Rot_Z_Ph1: Matrix( 

[ c(phi). -s(phi). 0. 0], 

[ s(phi). c(phi). 0. 0], 

[ 0. 0. 1. 0], 

[ 0. 0. 0. 1]); 

Euler_temp: Rot_Y_Theta . Rot_Z_Psi; 

Euler_matrix: Rot_Z_Phi . Euler.temp: 

homogenize(X):- [x[l]. x[Z]. x[3]. 1]; 

UnHomogenize(X) :- [x[l]/x[4]. x[2]/x[4], x[3]/x[4]]; 

Rotate_vector(X) :• UnHomogen1ze( Euler.Matrix . Homogenize(X)) ; 

/• Cross Product •/ 

Cross(A.B) :• 
C ("[Zl'bCa] - a[3]»b[2]). 
(a[3]»b[l] - a[l]«b[3]). / 

(a[l]»b[2] - a[2]'b[l])1; f 

/• Simplify if possible? •/ 

S1mp_l(Exp. Var) :• ratsubst(l. sin(var)-2 ♦ cos(var)*2. txp); 
simp_3(Exp) :• Rat( 

Simp_l( S1mp_l( S1mp_l(Exp. Ph1). Thata). Psi)); 

/• Genaral Transformation function. A 4-vector 1s assumed to be a plant, 
and a 3-vector a 3-wector. hence the 3-rector is ROTATED and a 4-vector 
plane is also rotated... See PAUL */ 

Transform(Transformation_matrix, X) :• 
Block([Hom, Trans. ans], 
Ans: "Whoops I". 
if length(X) - 4 then 

Ans: x .Transformation_matr1x 

Else 

if length (x) ■ 3 then 

Ans:Unhomogenize(Transformat1on_matrix . Homogenize (X)) 
Else Print{" But ",x." Must be a 3-wector or 4-Vectorl"), 

Ans); 

/• Now Compute Inverse of the Euler Transformation "/ 
E_Adj: simp_3(rat (adjoint( Euler_matr1x))); 
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